diff --git a/pps_ws/src/d_fall_pps/GUI_Qt/studentGUI/include/MainWindow.h b/pps_ws/src/d_fall_pps/GUI_Qt/studentGUI/include/MainWindow.h
index ca47a90e86023e0e325567ee2cc369b726c113d3..a9dcd7e86fb7ac454a3d40abfce3df64f1db5777 100644
--- a/pps_ws/src/d_fall_pps/GUI_Qt/studentGUI/include/MainWindow.h
+++ b/pps_ws/src/d_fall_pps/GUI_Qt/studentGUI/include/MainWindow.h
@@ -65,6 +65,7 @@ private slots:
void on_motors_OFF_button_clicked();
void on_set_setpoint_button_clicked();
+ void on_set_setpoint_button_2_clicked();
void on_pushButton_3_clicked();
@@ -90,7 +91,8 @@ private:
int m_student_id;
CrazyflieContext m_context;
- Setpoint m_setpoint;
+ Setpoint m_safe_setpoint;
+ Setpoint m_custom_setpoint;
ros::Publisher crazyRadioCommandPublisher;
ros::Subscriber crazyRadioStatusSubscriber;
@@ -99,7 +101,10 @@ private:
ros::Subscriber flyingStateSubscriber;
ros::Publisher controllerSetpointPublisher;
- ros::Subscriber setpointSubscriber;
+ ros::Subscriber safeSetpointSubscriber;
+
+ ros::Publisher customSetpointPublisher;
+ ros::Subscriber customSetpointSubscriber;
ros::Subscriber DBChangedSubscriber;
@@ -112,7 +117,8 @@ private:
void crazyRadioStatusCallback(const std_msgs::Int32& msg);
void CFBatteryCallback(const std_msgs::Float32& msg);
void flyingStateChangedCallback(const std_msgs::Int32& msg);
- void setpointCallback(const Setpoint& newSetpoint);
+ void safeSetpointCallback(const Setpoint& newSetpoint);
+ void customSetpointCallback(const Setpoint& newSetpoint);
void DBChangedCallback(const std_msgs::Int32& msg);
void customYamlFileTimerCallback(const ros::TimerEvent&);
void safeYamlFileTimerCallback(const ros::TimerEvent&);
diff --git a/pps_ws/src/d_fall_pps/GUI_Qt/studentGUI/src/MainWindow.cpp b/pps_ws/src/d_fall_pps/GUI_Qt/studentGUI/src/MainWindow.cpp
index 2e1678e7d82a9ba6e6d6db1294783b11f4d64ebd..891e14df23cd773b51cdb879709fa1f1438b3c2d 100644
--- a/pps_ws/src/d_fall_pps/GUI_Qt/studentGUI/src/MainWindow.cpp
+++ b/pps_ws/src/d_fall_pps/GUI_Qt/studentGUI/src/MainWindow.cpp
@@ -31,6 +31,9 @@ MainWindow::MainWindow(int argc, char **argv, QWidget *parent) :
ros::NodeHandle nodeHandle(m_ros_namespace);
+ customSetpointPublisher = nodeHandle.advertise<Setpoint>("CustomControllerService/Setpoint", 1);
+ customSetpointSubscriber = nodeHandle.subscribe("CustomControllerService/Setpoint", 1, &MainWindow::customSetpointCallback, this);
+
// subscribers
crazyRadioStatusSubscriber = nodeHandle.subscribe("CrazyRadio/CrazyRadioStatus", 1, &MainWindow::crazyRadioStatusCallback, this);
@@ -39,7 +42,7 @@ MainWindow::MainWindow(int argc, char **argv, QWidget *parent) :
flyingStateSubscriber = nodeHandle.subscribe("PPSClient/flyingState", 1, &MainWindow::flyingStateChangedCallback, this);
- setpointSubscriber = nodeHandle.subscribe("SafeControllerService/Setpoint", 1, &MainWindow::setpointCallback, this);
+ safeSetpointSubscriber = nodeHandle.subscribe("SafeControllerService/Setpoint", 1, &MainWindow::safeSetpointCallback, this);
DBChangedSubscriber = nodeHandle.subscribe("/my_GUI/DBChanged", 1, &MainWindow::DBChangedCallback, this);
ros::NodeHandle my_nodeHandle("~");
@@ -114,9 +117,9 @@ void MainWindow::DBChangedCallback(const std_msgs::Int32& msg)
ROS_INFO("context reloaded in student_GUI");
}
-void MainWindow::setpointCallback(const Setpoint& newSetpoint)
+void MainWindow::safeSetpointCallback(const Setpoint& newSetpoint)
{
- m_setpoint = newSetpoint;
+ m_safe_setpoint = newSetpoint;
// here we get the new setpoint, need to update it in GUI
ui->current_setpoint_x->setText(QString::number(newSetpoint.x, 'f', 3));
ui->current_setpoint_y->setText(QString::number(newSetpoint.y, 'f', 3));
@@ -124,6 +127,16 @@ void MainWindow::setpointCallback(const Setpoint& newSetpoint)
ui->current_setpoint_yaw->setText(QString::number(newSetpoint.yaw * RAD2DEG, 'f', 1));
}
+void MainWindow::customSetpointCallback(const Setpoint& newSetpoint)
+{
+ m_custom_setpoint = newSetpoint;
+ // here we get the new setpoint, need to update it in GUI
+ ui->current_setpoint_x_2->setText(QString::number(newSetpoint.x, 'f', 3));
+ ui->current_setpoint_y_2->setText(QString::number(newSetpoint.y, 'f', 3));
+ ui->current_setpoint_z_2->setText(QString::number(newSetpoint.z, 'f', 3));
+ ui->current_setpoint_yaw_2->setText(QString::number(newSetpoint.yaw * RAD2DEG, 'f', 1));
+}
+
void MainWindow::flyingStateChangedCallback(const std_msgs::Int32& msg)
{
QString qstr = "Flying State: ";
@@ -280,11 +293,23 @@ void MainWindow::updateNewViconData(const ptrToMessage& p_msg) //connected to ne
ui->current_pitch->setText(QString::number(local.pitch * RAD2DEG, 'f', 1));
ui->current_roll->setText(QString::number(local.roll * RAD2DEG, 'f', 1));
+ ui->current_x_2->setText(QString::number(local.x, 'f', 3));
+ ui->current_y_2->setText(QString::number(local.y, 'f', 3));
+ ui->current_z_2->setText(QString::number(local.z, 'f', 3));
+ ui->current_yaw_2->setText(QString::number(local.yaw * RAD2DEG, 'f', 1));
+ ui->current_pitch_2->setText(QString::number(local.pitch * RAD2DEG, 'f', 1));
+ ui->current_roll_2->setText(QString::number(local.roll * RAD2DEG, 'f', 1));
+
// also update diff
- ui->diff_x->setText(QString::number(m_setpoint.x - local.x, 'f', 3));
- ui->diff_y->setText(QString::number(m_setpoint.y - local.y, 'f', 3));
- ui->diff_z->setText(QString::number(m_setpoint.z - local.z, 'f', 3));
- ui->diff_yaw->setText(QString::number((m_setpoint.yaw - local.yaw) * RAD2DEG, 'f', 1));
+ ui->diff_x->setText(QString::number(m_safe_setpoint.x - local.x, 'f', 3));
+ ui->diff_y->setText(QString::number(m_safe_setpoint.y - local.y, 'f', 3));
+ ui->diff_z->setText(QString::number(m_safe_setpoint.z - local.z, 'f', 3));
+ ui->diff_yaw->setText(QString::number((m_safe_setpoint.yaw - local.yaw) * RAD2DEG, 'f', 1));
+
+ ui->diff_x_2->setText(QString::number(m_custom_setpoint.x - local.x, 'f', 3));
+ ui->diff_y_2->setText(QString::number(m_custom_setpoint.y - local.y, 'f', 3));
+ ui->diff_z_2->setText(QString::number(m_custom_setpoint.z - local.z, 'f', 3));
+ ui->diff_yaw_2->setText(QString::number((m_custom_setpoint.yaw - local.yaw) * RAD2DEG, 'f', 1));
}
}
}
@@ -329,6 +354,17 @@ void MainWindow::on_set_setpoint_button_clicked()
this->controllerSetpointPublisher.publish(msg_setpoint);
}
+void MainWindow::on_set_setpoint_button_2_clicked()
+{
+ Setpoint msg_setpoint;
+ msg_setpoint.x = (ui->new_setpoint_x_2->text()).toFloat();
+ msg_setpoint.y = (ui->new_setpoint_y_2->text()).toFloat();
+ msg_setpoint.z = (ui->new_setpoint_z_2->text()).toFloat();
+ msg_setpoint.yaw = (ui->new_setpoint_yaw_2->text()).toFloat() * DEG2RAD;
+
+ this->customSetpointPublisher.publish(msg_setpoint);
+}
+
void MainWindow::on_pushButton_3_clicked()
{
std_msgs::Int32 msg;
diff --git a/pps_ws/src/d_fall_pps/GUI_Qt/studentGUI/src/MainWindow.ui b/pps_ws/src/d_fall_pps/GUI_Qt/studentGUI/src/MainWindow.ui
index 69c5a8d204fd1035aaa7401a587d40e3149da4df..031cb6a45e12d377e494a3a2cc40a6e520dbfd74 100644
--- a/pps_ws/src/d_fall_pps/GUI_Qt/studentGUI/src/MainWindow.ui
+++ b/pps_ws/src/d_fall_pps/GUI_Qt/studentGUI/src/MainWindow.ui
@@ -45,7 +45,7 @@
<item row="1" column="0">
<widget class="QTabWidget" name="tabWidget">
<property name="currentIndex">
- <number>0</number>
+ <number>1</number>
</property>
<widget class="QWidget" name="tab_3">
<attribute name="title">
@@ -329,7 +329,7 @@
<item row="1" column="0">
<widget class="QPushButton" name="load_safe_yaml_button">
<property name="text">
- <string>Load YAML file</string>
+ <string>Load Safecontroller YAML file</string>
</property>
</widget>
</item>
@@ -617,7 +617,7 @@
<item row="1" column="0">
<widget class="QPushButton" name="load_custom_yaml_button">
<property name="text">
- <string>Load YAML file</string>
+ <string>Load CustomController YAML file</string>
</property>
</widget>
</item>
diff --git a/pps_ws/src/d_fall_pps/GUI_Qt/studentGUI/studentGUI.pro.user b/pps_ws/src/d_fall_pps/GUI_Qt/studentGUI/studentGUI.pro.user
index 684c0e671560c69ec8261627c3c8cf98bdd9acdf..3b064d2339f42557e51b032dbc9c7d731163b999 100644
--- a/pps_ws/src/d_fall_pps/GUI_Qt/studentGUI/studentGUI.pro.user
+++ b/pps_ws/src/d_fall_pps/GUI_Qt/studentGUI/studentGUI.pro.user
@@ -1,6 +1,6 @@
<?xml version="1.0" encoding="UTF-8"?>
<!DOCTYPE QtCreatorProject>
-<!-- Written by QtCreator 4.0.2, 2017-09-01T14:47:27. -->
+<!-- Written by QtCreator 4.0.2, 2017-09-05T11:57:14. -->
<qtcreator>
<data>
<variable>EnvironmentId</variable>
diff --git a/pps_ws/src/d_fall_pps/src/CustomControllerService.cpp b/pps_ws/src/d_fall_pps/src/CustomControllerService.cpp
index a3b8aa6eecff16dc2789e4bed80e4ea7d5d51884..e1fd96f12788d0f4dbd9912b94900980d2d82cc6 100644
--- a/pps_ws/src/d_fall_pps/src/CustomControllerService.cpp
+++ b/pps_ws/src/d_fall_pps/src/CustomControllerService.cpp
@@ -124,11 +124,21 @@ void customYAMLloadedCallback(const std_msgs::Int32& msg)
loadCustomParameters(nodeHandle);
}
+void setpointCallback(const Setpoint& newSetpoint) {
+ // setpoint[0] = newSetpoint.x;
+ // setpoint[1] = newSetpoint.y;
+ // setpoint[2] = newSetpoint.z;
+ // setpoint[3] = newSetpoint.yaw;
+}
+
int main(int argc, char* argv[]) {
//starting the ROS-node
ros::init(argc, argv, "CustomControllerService");
ros::NodeHandle nodeHandle("~");
+
+ ros::Subscriber setpointSubscriber = nodeHandle.subscribe("Setpoint", 1, setpointCallback);
+
ros::ServiceServer service = nodeHandle.advertiseService("CustomController", calculateControlOutput);
ros::NodeHandle namespace_nodeHandle(ros::this_node::getNamespace());
diff --git a/pps_ws/src/d_fall_pps/src/SafeControllerService.cpp b/pps_ws/src/d_fall_pps/src/SafeControllerService.cpp
index 6f1000d9d6a45100328c782dcc37c5f1af22fa56..b04cc185d88d6e83e567cb84591a9976b6ef791c 100755
--- a/pps_ws/src/d_fall_pps/src/SafeControllerService.cpp
+++ b/pps_ws/src/d_fall_pps/src/SafeControllerService.cpp
@@ -284,7 +284,6 @@ int main(int argc, char* argv[]) {
loadSafeParameters(nodeHandle);
setpoint = defaultSetpoint; // only first time setpoint is equal to default setpoint
- ros::Publisher setpointPublisher = nodeHandle.advertise<Setpoint>("Setpoint", 1);
ros::Subscriber setpointSubscriber = nodeHandle.subscribe("Setpoint", 1, setpointCallback);
ros::NodeHandle namespace_nodeHandle(ros::this_node::getNamespace());