Commit 1a3f0614 authored by roangel's avatar roangel
Browse files

Added custom setpoint, filled second tab with info

parent b5aa168f
......@@ -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&);
......
......@@ -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;
......
......@@ -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>
......
<?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>
......
......@@ -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());
......
......@@ -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());
......
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