From 70ad70b24fe097a37f07774df9cdcce597b67972 Mon Sep 17 00:00:00 2001
From: roangel <roangel@student.ethz.ch>
Date: Mon, 4 Sep 2017 11:49:10 +0200
Subject: [PATCH] Need to add load_safe_yaml_button to safe controller
---
.../GUI_Qt/studentGUI/include/MainWindow.h | 11 ++-
.../GUI_Qt/studentGUI/src/MainWindow.cpp | 79 ++++++++++++++-----
.../GUI_Qt/studentGUI/src/MainWindow.ui | 2 +-
pps_ws/src/d_fall_pps/launch/Student.launch | 10 +--
pps_ws/src/d_fall_pps/src/PPSClient.cpp | 26 ++++++
.../d_fall_pps/src/SafeControllerService.cpp | 29 ++++++-
6 files changed, 126 insertions(+), 31 deletions(-)
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 63cad337..b7d297d0 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
@@ -68,7 +68,8 @@ private slots:
void on_pushButton_3_clicked();
- void on_load_yaml_button_clicked();
+ void on_load_custom_yaml_button_clicked();
+ void on_load_safe_yaml_button_clicked();
void on_en_custom_controller_clicked();
@@ -84,7 +85,7 @@ private:
std::string m_ros_namespace;
- ros::Timer m_timer_yaml_file;
+ ros::Timer m_custom_timer_yaml_file;
int m_student_id;
CrazyflieContext m_context;
@@ -102,6 +103,9 @@ private:
ros::Subscriber DBChangedSubscriber;
+ ros::Publisher customYAMLloadedPublisher;
+ ros::Publisher safeYAMLloadedPublisher;
+
ros::ServiceClient centralManager;
// callbacks
@@ -110,7 +114,8 @@ private:
void flyingStateChangedCallback(const std_msgs::Int32& msg);
void setpointCallback(const Setpoint& newSetpoint);
void DBChangedCallback(const std_msgs::Int32& msg);
- void yamlFileTimerCallback(const ros::TimerEvent&);
+ void customYamlFileTimerCallback(const ros::TimerEvent&);
+ void safeYamlFileTimerCallback(const ros::TimerEvent&);
float fromVoltageToPercent(float voltage);
void updateBatteryVoltage(float battery_voltage);
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 db52fb06..33ba1e8f 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
@@ -42,6 +42,10 @@ MainWindow::MainWindow(int argc, char **argv, QWidget *parent) :
setpointSubscriber = nodeHandle.subscribe("SafeControllerService/Setpoint", 1, &MainWindow::setpointCallback, this);
DBChangedSubscriber = nodeHandle.subscribe("/my_GUI/DBChanged", 1, &MainWindow::DBChangedCallback, this);
+ ros::NodeHandle my_nodeHandle("~");
+ customYAMLloadedPublisher = my_nodeHandle.advertise<std_msgs::Int32>("customYAMLloaded", 1);
+ safeYAMLloadedPublisher = my_nodeHandle.advertise<std_msgs::Int32>("safeYAMLloaded", 1);
+
// communication with PPS Client, just to make it possible to communicate through terminal also we use PPSClient's name
ros::NodeHandle nh_PPSClient(m_ros_namespace + "/PPSClient");
@@ -49,6 +53,7 @@ MainWindow::MainWindow(int argc, char **argv, QWidget *parent) :
PPSClientCommandPublisher = nh_PPSClient.advertise<std_msgs::Int32>("Command", 1);
+
// First get student ID
if(!nh_PPSClient.getParam("studentID", m_student_id))
{
@@ -69,9 +74,12 @@ MainWindow::MainWindow(int argc, char **argv, QWidget *parent) :
std::vector<float> default_setpoint(4);
ros::NodeHandle nh_safeControllerService(m_ros_namespace + "/SafeControllerService");
- if(!nh_safeControllerService.getParam("defaultSetPoint", default_setpoint))
+
+ ROS_INFO_STREAM(m_ros_namespace << "/SafeControllerService");
+
+ if(!nh_safeControllerService.getParam("defaultSetpoint", default_setpoint))
{
- ROS_ERROR_STREAM("couldn't find parameter 'defaultSetPoint'");
+ ROS_ERROR_STREAM("couldn't find parameter 'defaultSetpoint'");
}
@@ -109,10 +117,10 @@ void MainWindow::setpointCallback(const Setpoint& newSetpoint)
{
m_setpoint = newSetpoint;
// here we get the new setpoint, need to update it in GUI
- ui->current_setpoint_x->setText(QString::number(newSetpoint.x));
- ui->current_setpoint_y->setText(QString::number(newSetpoint.y));
- ui->current_setpoint_z->setText(QString::number(newSetpoint.z));
- ui->current_setpoint_yaw->setText(QString::number(newSetpoint.yaw * RAD2DEG));
+ ui->current_setpoint_x->setText(QString::number(newSetpoint.x, 'f', 3));
+ ui->current_setpoint_y->setText(QString::number(newSetpoint.y, 'f', 3));
+ ui->current_setpoint_z->setText(QString::number(newSetpoint.z, 'f', 3));
+ ui->current_setpoint_yaw->setText(QString::number(newSetpoint.yaw * RAD2DEG, 'f', 1));
}
void MainWindow::flyingStateChangedCallback(const std_msgs::Int32& msg)
@@ -267,15 +275,15 @@ void MainWindow::updateNewViconData(const ptrToMessage& p_msg) //connected to ne
ui->current_x->setText(QString::number(local.x, 'f', 3));
ui->current_y->setText(QString::number(local.y, 'f', 3));
ui->current_z->setText(QString::number(local.z, 'f', 3));
- ui->current_yaw->setText(QString::number(local.yaw * RAD2DEG, 'f', 2));
- ui->current_pitch->setText(QString::number(local.pitch * RAD2DEG, 'f', 2));
- ui->current_roll->setText(QString::number(local.roll * RAD2DEG, 'f', 2));
+ ui->current_yaw->setText(QString::number(local.yaw * RAD2DEG, 'f', 1));
+ ui->current_pitch->setText(QString::number(local.pitch * RAD2DEG, 'f', 1));
+ ui->current_roll->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', 2));
+ ui->diff_yaw->setText(QString::number((m_setpoint.yaw - local.yaw) * RAD2DEG, 'f', 1));
}
}
}
@@ -328,31 +336,66 @@ void MainWindow::on_pushButton_3_clicked()
ROS_INFO("command disconnect published");
}
-void MainWindow::yamlFileTimerCallback(const ros::TimerEvent&)
+void MainWindow::safeYamlFileTimerCallback(const ros::TimerEvent&)
+{
+ // send msg that says that parameters have changed in YAML file
+ std_msgs::Int32 msg;
+ msg.data = 1;
+ this->safeYAMLloadedPublisher.publish(msg);
+ ROS_INFO("YALMloaded published");
+ ui->load_safe_yaml_button->setEnabled(true);
+}
+
+void MainWindow::on_load_safe_yaml_button_clicked()
{
+ ui->load_safe_yaml_button->setEnabled(false);
+ ros::NodeHandle nodeHandle("~");
+ m_custom_timer_yaml_file = nodeHandle.createTimer(ros::Duration(1), &MainWindow::safeYamlFileTimerCallback, this, true);
std::string d_fall_pps_path = ros::package::getPath("d_fall_pps");
ROS_INFO_STREAM(d_fall_pps_path);
// first, reload the name of the custom controller:
std::string cmd = "rosparam load " + d_fall_pps_path + "/param/ClientConfig.yaml " + m_ros_namespace + "/PPSClient";
-
system(cmd.c_str());
ROS_INFO_STREAM(cmd);
// then, reload the parameters of the custom controller:
- cmd = "rosparam load " + d_fall_pps_path + "/param/CustomController.yaml " + m_ros_namespace + "/CustomControllerService";
-
+ cmd = "rosparam load " + d_fall_pps_path + "/param/SafeController.yaml " + m_ros_namespace + "/SafeControllerService";
system(cmd.c_str());
ROS_INFO_STREAM(cmd);
- ui->load_yaml_button->setEnabled(true);
}
-void MainWindow::on_load_yaml_button_clicked()
+
+
+void MainWindow::customYamlFileTimerCallback(const ros::TimerEvent&)
{
+ // send msg that says that parameters have changed in YAML file
+ std_msgs::Int32 msg;
+ msg.data = 1;
+ this->customYAMLloadedPublisher.publish(msg);
+ ROS_INFO("YALMloaded published");
+ ui->load_custom_yaml_button->setEnabled(true);
+}
+
+void MainWindow::on_load_custom_yaml_button_clicked()
+{
+ ui->load_custom_yaml_button->setEnabled(false);
ros::NodeHandle nodeHandle("~");
- m_timer_yaml_file = nodeHandle.createTimer(ros::Duration(0.5), &MainWindow::yamlFileTimerCallback, this, true);
- ui->load_yaml_button->setEnabled(false);
+ m_custom_timer_yaml_file = nodeHandle.createTimer(ros::Duration(1), &MainWindow::customYamlFileTimerCallback, this, true);
+
+ std::string d_fall_pps_path = ros::package::getPath("d_fall_pps");
+ ROS_INFO_STREAM(d_fall_pps_path);
+
+ // first, reload the name of the custom controller:
+ std::string cmd = "rosparam load " + d_fall_pps_path + "/param/ClientConfig.yaml " + m_ros_namespace + "/PPSClient";
+ system(cmd.c_str());
+ ROS_INFO_STREAM(cmd);
+
+ // then, reload the parameters of the custom controller:
+ cmd = "rosparam load " + d_fall_pps_path + "/param/CustomController.yaml " + m_ros_namespace + "/CustomControllerService";
+ system(cmd.c_str());
+ ROS_INFO_STREAM(cmd);
}
void MainWindow::on_en_custom_controller_clicked()
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 e0f50cd1..e0bf6fba 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
@@ -608,7 +608,7 @@
</widget>
</item>
<item row="1" column="0">
- <widget class="QPushButton" name="load_yaml_button">
+ <widget class="QPushButton" name="load_custom_yaml_button">
<property name="text">
<string>Load YAML file</string>
</property>
diff --git a/pps_ws/src/d_fall_pps/launch/Student.launch b/pps_ws/src/d_fall_pps/launch/Student.launch
index fb7a91d3..475bf792 100755
--- a/pps_ws/src/d_fall_pps/launch/Student.launch
+++ b/pps_ws/src/d_fall_pps/launch/Student.launch
@@ -1,10 +1,5 @@
<launch>
- <!-- When we have a GUI, this has to be adapted and included -->
- <node pkg="d_fall_pps" name="student_GUI" output="screen" type="student_GUI">
- <!-- <rosparam command="load" file="$(find d_fall_pps)/launch/ControlParams.yaml" /> -->
- </node>
-
<node pkg="d_fall_pps" name="CrazyRadio" output="screen" type="CrazyRadio.py">
<rosparam command="load" file="$(find d_fall_pps)/param/ClientConfig.yaml" />
</node>
@@ -21,6 +16,11 @@
<node pkg="d_fall_pps" name="CustomControllerService" output="screen" type="CustomControllerService">
</node>
+ <!-- When we have a GUI, this has to be adapted and included -->
+ <node pkg="d_fall_pps" name="student_GUI" output="screen" type="student_GUI">
+ <!-- <rosparam command="load" file="$(find d_fall_pps)/launch/ControlParams.yaml" /> -->
+ </node>
+
</launch>
diff --git a/pps_ws/src/d_fall_pps/src/PPSClient.cpp b/pps_ws/src/d_fall_pps/src/PPSClient.cpp
index 4bfcbed9..238b62d0 100755
--- a/pps_ws/src/d_fall_pps/src/PPSClient.cpp
+++ b/pps_ws/src/d_fall_pps/src/PPSClient.cpp
@@ -107,6 +107,8 @@ CrazyflieContext context;
//wheter to use safe of custom controller
bool usingSafeController;
+std::string ros_namespace;
+
void loadSafeController() {
ros::NodeHandle nodeHandle("~");
@@ -258,6 +260,27 @@ void landTimerCallback(const ros::TimerEvent&)
finished_land = true;
}
+void goToDefaultSetpoint()
+{
+ std::vector<float> default_setpoint(4);
+ ros::NodeHandle nh_safeControllerService(ros_namespace + "/SafeControllerService");
+
+ ROS_INFO_STREAM(ros_namespace << "/SafeControllerService");
+
+ if(!nh_safeControllerService.getParam("defaultSetpoint", default_setpoint))
+ {
+ ROS_ERROR_STREAM("couldn't find parameter 'defaultSetpoint'");
+ }
+
+ Setpoint setpoint_msg;
+ setpoint_msg.x = default_setpoint[0];
+ setpoint_msg.y = default_setpoint[1];
+ setpoint_msg.z = default_setpoint[2];
+ ROS_INFO_STREAM("Z =" << default_setpoint[2]);
+ setpoint_msg.yaw = default_setpoint[3];
+ safeControllerServiceSetpointPublisher.publish(setpoint_msg);
+}
+
//is called when new data from Vicon arrives
void viconCallback(const ViconData& viconData) {
for(std::vector<CrazyflieData>::const_iterator it = viconData.crazyflies.begin(); it != viconData.crazyflies.end(); ++it) {
@@ -301,6 +324,7 @@ void viconCallback(const ViconData& viconData) {
{
changed_state_flag = false;
// need to change setpoint to the one from file
+ goToDefaultSetpoint();
ROS_INFO("STATE_FLYING");
}
break;
@@ -482,6 +506,8 @@ int main(int argc, char* argv[])
{
ros::init(argc, argv, "PPSClient");
ros::NodeHandle nodeHandle("~");
+ ros_namespace = ros::this_node::getNamespace();
+
loadParameters(nodeHandle);
//ros::service::waitForService("/CentralManagerService/CentralManager");
diff --git a/pps_ws/src/d_fall_pps/src/SafeControllerService.cpp b/pps_ws/src/d_fall_pps/src/SafeControllerService.cpp
index 568f11d4..f0f7e5b4 100755
--- a/pps_ws/src/d_fall_pps/src/SafeControllerService.cpp
+++ b/pps_ws/src/d_fall_pps/src/SafeControllerService.cpp
@@ -28,6 +28,8 @@
#include "d_fall_pps/Controller.h"
#include "d_fall_pps/Debugging.h" //---------------------------------------------------------------------------
+#include <std_msgs/Int32.h>
+
#define PI 3.1415926535
#define RATE_CONTROLLER 0
@@ -66,7 +68,7 @@ void loadParameterFloatVector(ros::NodeHandle& nodeHandle, std::string name, std
}
}
-void loadParameters(ros::NodeHandle& nodeHandle) {
+void loadSafeParameters(ros::NodeHandle& nodeHandle) {
loadParameterFloatVector(nodeHandle, "feedforwardMotor", feedforwardMotor, 4);
loadParameterFloatVector(nodeHandle, "motorPoly", motorPoly, 3);
@@ -162,8 +164,8 @@ void convertIntoBodyFrame(float est[9], float (&state)[9], float yaw_measured) {
bool calculateControlOutput(Controller::Request &request, Controller::Response &response)
{
- ros::NodeHandle nodeHandle("~");
- loadParameters(nodeHandle);
+ // ros::NodeHandle nodeHandle("~");
+ // loadSafeParameters(nodeHandle); // do not put this here, cannot control anymore
CrazyflieData vicon = request.ownCrazyflie;
@@ -265,6 +267,20 @@ void setpointCallback(const Setpoint& newSetpoint) {
setpoint[3] = newSetpoint.yaw;
}
+void customYAMLloadedCallback(const std_msgs::Int32& msg)
+{
+ ros::NodeHandle nodeHandle("~");
+ ROS_INFO("received msg custom loaded YAML");
+ // loadSafeParameters(nodeHandle);
+}
+
+void safeYAMLloadedCallback(const std_msgs::Int32& msg)
+{
+ ros::NodeHandle nodeHandle("~");
+ ROS_INFO("received msg safe loaded YAML");
+ loadSafeParameters(nodeHandle);
+}
+
//ros::Publisher pub;
@@ -272,12 +288,17 @@ int main(int argc, char* argv[]) {
ros::init(argc, argv, "SafeControllerService");
ros::NodeHandle nodeHandle("~");
- loadParameters(nodeHandle);
+ 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());
+
+ ros::Subscriber customYAMloadedSubscriber = namespace_nodeHandle.subscribe("student_GUI/customYAMLloaded", 1, customYAMLloadedCallback);
+ ros::Subscriber safeYAMloadedSubscriber = namespace_nodeHandle.subscribe("student_GUI/safeYAMLloaded", 1, safeYAMLloadedCallback);
+
ros::ServiceServer service = nodeHandle.advertiseService("RateController", calculateControlOutput);
ROS_INFO("SafeControllerService ready");
--
GitLab