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:
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);
......
......@@ -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()
......
......@@ -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>
......
<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>
......@@ -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");
......
......@@ -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");
......
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