From 1e87768dd0f65dbc4813b0cfd751f82856e4e874 Mon Sep 17 00:00:00 2001
From: roangel <roangel@student.ethz.ch>
Date: Fri, 1 Sep 2017 14:49:04 +0200
Subject: [PATCH] Need to test downstairs
---
.../GUI_Qt/studentGUI/include/MainWindow.h | 3 +
.../GUI_Qt/studentGUI/src/MainWindow.cpp | 71 +++++++++++--------
.../GUI_Qt/studentGUI/studentGUI.pro.user | 2 +-
.../d_fall_pps/param/CustomController.yaml | 0
pps_ws/src/d_fall_pps/src/PPSClient.cpp | 11 ---
.../d_fall_pps/src/SafeControllerService.cpp | 12 +++-
6 files changed, 53 insertions(+), 46 deletions(-)
create mode 100644 pps_ws/src/d_fall_pps/param/CustomController.yaml
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 b3aa5eeb..63cad337 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
@@ -84,6 +84,8 @@ private:
std::string m_ros_namespace;
+ ros::Timer m_timer_yaml_file;
+
int m_student_id;
CrazyflieContext m_context;
@@ -108,6 +110,7 @@ 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&);
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 8eacfed7..db52fb06 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
@@ -67,6 +67,19 @@ MainWindow::MainWindow(int argc, char **argv, QWidget *parent) :
qstr.append(QString::fromStdString(m_context.crazyflieName));
ui->groupBox->setTitle(qstr);
+ std::vector<float> default_setpoint(4);
+ ros::NodeHandle nh_safeControllerService(m_ros_namespace + "/SafeControllerService");
+ if(!nh_safeControllerService.getParam("defaultSetPoint", default_setpoint))
+ {
+ ROS_ERROR_STREAM("couldn't find parameter 'defaultSetPoint'");
+ }
+
+
+ ui->current_setpoint_x->setText(QString::number(default_setpoint[0]));
+ ui->current_setpoint_y->setText(QString::number(default_setpoint[1]));
+ ui->current_setpoint_z->setText(QString::number(default_setpoint[2]));
+ ui->current_setpoint_yaw->setText(QString::number(default_setpoint[3]));
+
disableGUI();
}
@@ -254,15 +267,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', 3));
- ui->current_pitch->setText(QString::number(local.pitch * RAD2DEG, 'f', 3));
- ui->current_roll->setText(QString::number(local.roll * RAD2DEG, '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));
// 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', 3));
+ ui->diff_yaw->setText(QString::number((m_setpoint.yaw - local.yaw) * RAD2DEG, 'f', 2));
}
}
}
@@ -315,28 +328,38 @@ void MainWindow::on_pushButton_3_clicked()
ROS_INFO("command disconnect published");
}
-void MainWindow::on_load_yaml_button_clicked()
+void MainWindow::yamlFileTimerCallback(const ros::TimerEvent&)
{
-
+
+ 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);
+ ui->load_yaml_button->setEnabled(true);
}
-void loadParameterFloatVector(ros::NodeHandle& nodeHandle, std::string name, std::vector<float>& val, int length)
+void MainWindow::on_load_yaml_button_clicked()
{
- if(!nodeHandle.getParam(name, val))
- {
- ROS_ERROR_STREAM("missing parameter '" << name << "'");
- }
- if(val.size() != length)
- {
- ROS_ERROR_STREAM("parameter '" << name << "' has wrong array length, " << length << " needed");
- }
+ ros::NodeHandle nodeHandle("~");
+ m_timer_yaml_file = nodeHandle.createTimer(ros::Duration(0.5), &MainWindow::yamlFileTimerCallback, this, true);
+ ui->load_yaml_button->setEnabled(false);
}
void MainWindow::on_en_custom_controller_clicked()
{
std_msgs::Int32 msg;
msg.data = CMD_USE_CUSTOM_CONTROLLER;
- // this->PPSClientCommandPublisher.publish(msg);
+ this->PPSClientCommandPublisher.publish(msg);
}
@@ -344,19 +367,5 @@ void MainWindow::on_en_safe_controller_clicked()
{
std_msgs::Int32 msg;
msg.data = CMD_USE_SAFE_CONTROLLER;
-
-
- std::string d_fall_pps_path = ros::package::getPath("d_fall_pps");
- ROS_INFO_STREAM(d_fall_pps_path);
-
- std::string cmd = "rosparam load " + d_fall_pps_path + "/param/SafeController.yaml " + m_ros_namespace + "/SafeControllerService";
-
- system(cmd.c_str());
- ROS_INFO_STREAM(cmd);
-
- std::vector<float> setpoint(4);
- ros::NodeHandle nodeHandle(m_ros_namespace + "/SafeControllerService");
- loadParameterFloatVector(nodeHandle, "defaultSetpoint", setpoint, 4);
- ROS_INFO_STREAM("setpoint z:" << setpoint[2]);
- // this->PPSClientCommandPublisher.publish(msg);
+ this->PPSClientCommandPublisher.publish(msg);
}
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 470dfb69..684c0e67 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-08-29T18:10:33. -->
+<!-- Written by QtCreator 4.0.2, 2017-09-01T14:47:27. -->
<qtcreator>
<data>
<variable>EnvironmentId</variable>
diff --git a/pps_ws/src/d_fall_pps/param/CustomController.yaml b/pps_ws/src/d_fall_pps/param/CustomController.yaml
new file mode 100644
index 00000000..e69de29b
diff --git a/pps_ws/src/d_fall_pps/src/PPSClient.cpp b/pps_ws/src/d_fall_pps/src/PPSClient.cpp
index db9d0580..4bfcbed9 100755
--- a/pps_ws/src/d_fall_pps/src/PPSClient.cpp
+++ b/pps_ws/src/d_fall_pps/src/PPSClient.cpp
@@ -223,16 +223,6 @@ void landCF(CrazyflieData& current_local_coordinates)
loadSafeController();
}
-void goToOrigin()
-{
- Setpoint setpoint_msg;
- setpoint_msg.x = 0;
- setpoint_msg.y = 0;
- setpoint_msg.z = 0.4;
- setpoint_msg.yaw = 0;
- safeControllerServiceSetpointPublisher.publish(setpoint_msg);
-}
-
void changeFlyingStateTo(int new_state)
{
if(crazyradio_status == CONNECTED)
@@ -311,7 +301,6 @@ void viconCallback(const ViconData& viconData) {
{
changed_state_flag = false;
// need to change setpoint to the one from file
- goToOrigin();
ROS_INFO("STATE_FLYING");
}
break;
diff --git a/pps_ws/src/d_fall_pps/src/SafeControllerService.cpp b/pps_ws/src/d_fall_pps/src/SafeControllerService.cpp
index ea4c5cc6..568f11d4 100755
--- a/pps_ws/src/d_fall_pps/src/SafeControllerService.cpp
+++ b/pps_ws/src/d_fall_pps/src/SafeControllerService.cpp
@@ -49,6 +49,7 @@ std::vector<float> estimatorMatrix(2);
float prevEstimate[9];
std::vector<float> setpoint(4);
+std::vector<float> defaultSetpoint(4);
float saturationThrust;
CrazyflieData previousLocation;
@@ -82,7 +83,7 @@ void loadParameters(ros::NodeHandle& nodeHandle) {
loadParameterFloatVector(nodeHandle, "filterGain", filterGain, 6);
loadParameterFloatVector(nodeHandle, "estimatorMatrix", estimatorMatrix, 2);
- loadParameterFloatVector(nodeHandle, "defaultSetpoint", setpoint, 4);
+ loadParameterFloatVector(nodeHandle, "defaultSetpoint", defaultSetpoint, 4);
}
float computeMotorPolyBackward(float thrust) {
@@ -159,7 +160,11 @@ void convertIntoBodyFrame(float est[9], float (&state)[9], float yaw_measured) {
state[8] = est[8];
}
-bool calculateControlOutput(Controller::Request &request, Controller::Response &response) {
+bool calculateControlOutput(Controller::Request &request, Controller::Response &response)
+{
+ ros::NodeHandle nodeHandle("~");
+ loadParameters(nodeHandle);
+
CrazyflieData vicon = request.ownCrazyflie;
float yaw_measured = request.ownCrazyflie.yaw;
@@ -249,7 +254,7 @@ bool calculateControlOutput(Controller::Request &request, Controller::Response &
previousLocation = request.ownCrazyflie;
bag.write("ControlOutput", ros::Time::now(), response.controlOutput);
-
+
return true;
}
@@ -268,6 +273,7 @@ int main(int argc, char* argv[]) {
ros::NodeHandle nodeHandle("~");
loadParameters(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);
--
GitLab