Commit 70ad70b2 authored by roangel's avatar roangel
Browse files

Need to add load_safe_yaml_button to safe controller

parent 1e87768d
...@@ -68,7 +68,8 @@ private slots: ...@@ -68,7 +68,8 @@ private slots:
void on_pushButton_3_clicked(); 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(); void on_en_custom_controller_clicked();
...@@ -84,7 +85,7 @@ private: ...@@ -84,7 +85,7 @@ private:
std::string m_ros_namespace; std::string m_ros_namespace;
ros::Timer m_timer_yaml_file; ros::Timer m_custom_timer_yaml_file;
int m_student_id; int m_student_id;
CrazyflieContext m_context; CrazyflieContext m_context;
...@@ -102,6 +103,9 @@ private: ...@@ -102,6 +103,9 @@ private:
ros::Subscriber DBChangedSubscriber; ros::Subscriber DBChangedSubscriber;
ros::Publisher customYAMLloadedPublisher;
ros::Publisher safeYAMLloadedPublisher;
ros::ServiceClient centralManager; ros::ServiceClient centralManager;
// callbacks // callbacks
...@@ -110,7 +114,8 @@ private: ...@@ -110,7 +114,8 @@ private:
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); 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); float fromVoltageToPercent(float voltage);
void updateBatteryVoltage(float battery_voltage); void updateBatteryVoltage(float battery_voltage);
......
...@@ -42,6 +42,10 @@ MainWindow::MainWindow(int argc, char **argv, QWidget *parent) : ...@@ -42,6 +42,10 @@ MainWindow::MainWindow(int argc, char **argv, QWidget *parent) :
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); 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 // 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"); ros::NodeHandle nh_PPSClient(m_ros_namespace + "/PPSClient");
...@@ -49,6 +53,7 @@ MainWindow::MainWindow(int argc, char **argv, QWidget *parent) : ...@@ -49,6 +53,7 @@ MainWindow::MainWindow(int argc, char **argv, QWidget *parent) :
PPSClientCommandPublisher = nh_PPSClient.advertise<std_msgs::Int32>("Command", 1); PPSClientCommandPublisher = nh_PPSClient.advertise<std_msgs::Int32>("Command", 1);
// First get student ID // First get student ID
if(!nh_PPSClient.getParam("studentID", m_student_id)) if(!nh_PPSClient.getParam("studentID", m_student_id))
{ {
...@@ -69,9 +74,12 @@ MainWindow::MainWindow(int argc, char **argv, QWidget *parent) : ...@@ -69,9 +74,12 @@ MainWindow::MainWindow(int argc, char **argv, QWidget *parent) :
std::vector<float> default_setpoint(4); std::vector<float> default_setpoint(4);
ros::NodeHandle nh_safeControllerService(m_ros_namespace + "/SafeControllerService"); 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) ...@@ -109,10 +117,10 @@ void MainWindow::setpointCallback(const Setpoint& newSetpoint)
{ {
m_setpoint = newSetpoint; m_setpoint = newSetpoint;
// here we get the new setpoint, need to update it in GUI // here we get the new setpoint, need to update it in GUI
ui->current_setpoint_x->setText(QString::number(newSetpoint.x)); ui->current_setpoint_x->setText(QString::number(newSetpoint.x, 'f', 3));
ui->current_setpoint_y->setText(QString::number(newSetpoint.y)); ui->current_setpoint_y->setText(QString::number(newSetpoint.y, 'f', 3));
ui->current_setpoint_z->setText(QString::number(newSetpoint.z)); ui->current_setpoint_z->setText(QString::number(newSetpoint.z, 'f', 3));
ui->current_setpoint_yaw->setText(QString::number(newSetpoint.yaw * RAD2DEG)); ui->current_setpoint_yaw->setText(QString::number(newSetpoint.yaw * RAD2DEG, 'f', 1));
} }
void MainWindow::flyingStateChangedCallback(const std_msgs::Int32& msg) void MainWindow::flyingStateChangedCallback(const std_msgs::Int32& msg)
...@@ -267,15 +275,15 @@ void MainWindow::updateNewViconData(const ptrToMessage& p_msg) //connected to ne ...@@ -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_x->setText(QString::number(local.x, 'f', 3));
ui->current_y->setText(QString::number(local.y, 'f', 3)); ui->current_y->setText(QString::number(local.y, 'f', 3));
ui->current_z->setText(QString::number(local.z, '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_yaw->setText(QString::number(local.yaw * RAD2DEG, 'f', 1));
ui->current_pitch->setText(QString::number(local.pitch * RAD2DEG, 'f', 2)); ui->current_pitch->setText(QString::number(local.pitch * RAD2DEG, 'f', 1));
ui->current_roll->setText(QString::number(local.roll * RAD2DEG, 'f', 2)); ui->current_roll->setText(QString::number(local.roll * RAD2DEG, 'f', 1));
// also update diff // also update diff
ui->diff_x->setText(QString::number(m_setpoint.x - local.x, 'f', 3)); 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_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_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() ...@@ -328,31 +336,66 @@ void MainWindow::on_pushButton_3_clicked()
ROS_INFO("command disconnect published"); 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"); std::string d_fall_pps_path = ros::package::getPath("d_fall_pps");
ROS_INFO_STREAM(d_fall_pps_path); ROS_INFO_STREAM(d_fall_pps_path);
// first, reload the name of the custom controller: // first, reload the name of the custom controller:
std::string cmd = "rosparam load " + d_fall_pps_path + "/param/ClientConfig.yaml " + m_ros_namespace + "/PPSClient"; std::string cmd = "rosparam load " + d_fall_pps_path + "/param/ClientConfig.yaml " + m_ros_namespace + "/PPSClient";
system(cmd.c_str()); system(cmd.c_str());
ROS_INFO_STREAM(cmd); ROS_INFO_STREAM(cmd);
// then, reload the parameters of the custom controller: // 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()); system(cmd.c_str());
ROS_INFO_STREAM(cmd); 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("~"); ros::NodeHandle nodeHandle("~");
m_timer_yaml_file = nodeHandle.createTimer(ros::Duration(0.5), &MainWindow::yamlFileTimerCallback, this, true); m_custom_timer_yaml_file = nodeHandle.createTimer(ros::Duration(1), &MainWindow::customYamlFileTimerCallback, this, true);
ui->load_yaml_button->setEnabled(false);
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() void MainWindow::on_en_custom_controller_clicked()
......
...@@ -608,7 +608,7 @@ ...@@ -608,7 +608,7 @@
</widget> </widget>
</item> </item>
<item row="1" column="0"> <item row="1" column="0">
<widget class="QPushButton" name="load_yaml_button"> <widget class="QPushButton" name="load_custom_yaml_button">
<property name="text"> <property name="text">
<string>Load YAML file</string> <string>Load YAML file</string>
</property> </property>
......
<launch> <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"> <node pkg="d_fall_pps" name="CrazyRadio" output="screen" type="CrazyRadio.py">
<rosparam command="load" file="$(find d_fall_pps)/param/ClientConfig.yaml" /> <rosparam command="load" file="$(find d_fall_pps)/param/ClientConfig.yaml" />
</node> </node>
...@@ -21,6 +16,11 @@ ...@@ -21,6 +16,11 @@
<node pkg="d_fall_pps" name="CustomControllerService" output="screen" type="CustomControllerService"> <node pkg="d_fall_pps" name="CustomControllerService" output="screen" type="CustomControllerService">
</node> </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> </launch>
...@@ -107,6 +107,8 @@ CrazyflieContext context; ...@@ -107,6 +107,8 @@ CrazyflieContext context;
//wheter to use safe of custom controller //wheter to use safe of custom controller
bool usingSafeController; bool usingSafeController;
std::string ros_namespace;
void loadSafeController() { void loadSafeController() {
ros::NodeHandle nodeHandle("~"); ros::NodeHandle nodeHandle("~");
...@@ -258,6 +260,27 @@ void landTimerCallback(const ros::TimerEvent&) ...@@ -258,6 +260,27 @@ void landTimerCallback(const ros::TimerEvent&)
finished_land = true; 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 //is called when new data from Vicon arrives
void viconCallback(const ViconData& viconData) { void viconCallback(const ViconData& viconData) {
for(std::vector<CrazyflieData>::const_iterator it = viconData.crazyflies.begin(); it != viconData.crazyflies.end(); ++it) { for(std::vector<CrazyflieData>::const_iterator it = viconData.crazyflies.begin(); it != viconData.crazyflies.end(); ++it) {
...@@ -301,6 +324,7 @@ void viconCallback(const ViconData& viconData) { ...@@ -301,6 +324,7 @@ void viconCallback(const ViconData& viconData) {
{ {
changed_state_flag = false; changed_state_flag = false;
// need to change setpoint to the one from file // need to change setpoint to the one from file
goToDefaultSetpoint();
ROS_INFO("STATE_FLYING"); ROS_INFO("STATE_FLYING");
} }
break; break;
...@@ -482,6 +506,8 @@ int main(int argc, char* argv[]) ...@@ -482,6 +506,8 @@ int main(int argc, char* argv[])
{ {
ros::init(argc, argv, "PPSClient"); ros::init(argc, argv, "PPSClient");
ros::NodeHandle nodeHandle("~"); ros::NodeHandle nodeHandle("~");
ros_namespace = ros::this_node::getNamespace();
loadParameters(nodeHandle); loadParameters(nodeHandle);
//ros::service::waitForService("/CentralManagerService/CentralManager"); //ros::service::waitForService("/CentralManagerService/CentralManager");
......
...@@ -28,6 +28,8 @@ ...@@ -28,6 +28,8 @@
#include "d_fall_pps/Controller.h" #include "d_fall_pps/Controller.h"
#include "d_fall_pps/Debugging.h" //--------------------------------------------------------------------------- #include "d_fall_pps/Debugging.h" //---------------------------------------------------------------------------
#include <std_msgs/Int32.h>
#define PI 3.1415926535 #define PI 3.1415926535
#define RATE_CONTROLLER 0 #define RATE_CONTROLLER 0
...@@ -66,7 +68,7 @@ void loadParameterFloatVector(ros::NodeHandle& nodeHandle, std::string name, std ...@@ -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, "feedforwardMotor", feedforwardMotor, 4);
loadParameterFloatVector(nodeHandle, "motorPoly", motorPoly, 3); loadParameterFloatVector(nodeHandle, "motorPoly", motorPoly, 3);
...@@ -162,8 +164,8 @@ void convertIntoBodyFrame(float est[9], float (&state)[9], float yaw_measured) { ...@@ -162,8 +164,8 @@ void convertIntoBodyFrame(float est[9], float (&state)[9], float yaw_measured) {
bool calculateControlOutput(Controller::Request &request, Controller::Response &response) bool calculateControlOutput(Controller::Request &request, Controller::Response &response)
{ {
ros::NodeHandle nodeHandle("~"); // ros::NodeHandle nodeHandle("~");
loadParameters(nodeHandle); // loadSafeParameters(nodeHandle); // do not put this here, cannot control anymore
CrazyflieData vicon = request.ownCrazyflie; CrazyflieData vicon = request.ownCrazyflie;
...@@ -265,6 +267,20 @@ void setpointCallback(const Setpoint& newSetpoint) { ...@@ -265,6 +267,20 @@ void setpointCallback(const Setpoint& newSetpoint) {
setpoint[3] = newSetpoint.yaw; 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; //ros::Publisher pub;
...@@ -272,12 +288,17 @@ int main(int argc, char* argv[]) { ...@@ -272,12 +288,17 @@ int main(int argc, char* argv[]) {
ros::init(argc, argv, "SafeControllerService"); ros::init(argc, argv, "SafeControllerService");
ros::NodeHandle nodeHandle("~"); ros::NodeHandle nodeHandle("~");
loadParameters(nodeHandle); loadSafeParameters(nodeHandle);
setpoint = defaultSetpoint; // only first time setpoint is equal to default setpoint setpoint = defaultSetpoint; // only first time setpoint is equal to default setpoint
ros::Publisher setpointPublisher = nodeHandle.advertise<Setpoint>("Setpoint", 1); ros::Publisher setpointPublisher = nodeHandle.advertise<Setpoint>("Setpoint", 1);
ros::Subscriber setpointSubscriber = nodeHandle.subscribe("Setpoint", 1, setpointCallback); 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::ServiceServer service = nodeHandle.advertiseService("RateController", calculateControlOutput);
ROS_INFO("SafeControllerService ready"); ROS_INFO("SafeControllerService ready");
......
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