Skip to content

GitLab

Commit 574f6b06 authored by pragash1's avatar pragash1
Browse files

-Added new DroneX buttons to GUI. 

-Added DroneX yaml button to load parameters from yaml files.
-Added DroneX states: ground, take_off, hover, approach, land on mothership,
land on ground, on mothership
-Added onButtonPressed functions
-Deadlocks in onTakeOff, onLandOnMothership, OnAbort in DroneXControllerService
possible
parent 4fd208ed
Hide whitespace changes
Inline Side-by-side
pps_ws/src/d_fall_pps/GUI_Qt/studentGUI/include/MainWindow.h
View file @ 574f6b06
......@@ -111,7 +111,7 @@
#define LOAD_YAML_REMOTE_CONTROLLER_AGENT 5
#define LOAD_YAML_TUNING_CONTROLLER_AGENT 6
#define LOAD_YAML_PICKER_CONTROLLER_AGENT 7
#define LOAD_YAML_DRONEX_CONTROLLER_AGENT 8
#define LOAD_YAML_DRONEX_CONTROLLER_AGENT 8
#define LOAD_YAML_SAFE_CONTROLLER_COORDINATOR 11
#define LOAD_YAML_DEMO_CONTROLLER_COORDINATOR 12
......@@ -143,6 +143,7 @@
// FOR WHICH BUTTON WAS PRESSED IN THE DRONEX CONTOLLER
#define DRONEX_TAKE_OFF 8
#define DRONEX_LAND 9
#define DRONEX_ABORT 10
// Universal constants
......@@ -189,6 +190,7 @@ private slots:
void on_load_remote_yaml_button_clicked();
void on_load_tuning_yaml_button_clicked();
void on_load_picker_yaml_button_clicked();
void on_load_dronex_yaml_button_clicked();
// # Enable controllers
void on_enable_safe_controller_clicked();
......@@ -250,6 +252,7 @@ private slots:
// Interactions with the DroneX controller tab
void on_take_off_dronex_button_clicked();
void on_land_on_dronex_button_clicked();
void on_abort_dronex_button_clicked();
......@@ -275,6 +278,7 @@ private:
ros::Timer m_timer_yaml_file_for_remote_controller;
ros::Timer m_timer_yaml_file_for_tuning_controller;
ros::Timer m_timer_yaml_file_for_picker_controller;
ros::Timer m_timer_yaml_file_for_dronex_controller;
int m_student_id;
......@@ -415,7 +419,7 @@ private:
void remoteYamlFileTimerCallback(const ros::TimerEvent&);
void tuningYamlFileTimerCallback(const ros::TimerEvent&);
void pickerYamlFileTimerCallback(const ros::TimerEvent&);
void dronexYamlFileTimerCallback(const ros::TimerEvent&);
void requestLoadControllerYaml_from_my_GUI_Callback(const std_msgs::Int32& msg);
......
pps_ws/src/d_fall_pps/GUI_Qt/studentGUI/src/MainWindow.cpp
View file @ 574f6b06
......@@ -390,8 +390,9 @@ void MainWindow::controllerUsedChangedCallback(const std_msgs::Int32& msg)
break;
case DRONEX_CONTROLLER:
highlightDroneXControllerTab();
break;
default:
ROS_INFO("controllerUsedChangedCallback-Controller Flag not recognised");
ROS_INFO_STREAM("controllerUsedChangedCallback-Controller Flag not recognised----------------" << msg.data);
break;
}
}
......@@ -1170,6 +1171,69 @@ void MainWindow::safeYamlFileTimerCallback(const ros::TimerEvent&)
// Enble the "load safe yaml" button again
ui->load_safe_yaml_button->setEnabled(true);
}
// ----------------------------------------------------------------------------------
// # Load Yaml when acting as the GUI for an Agent
void MainWindow::on_load_dronex_yaml_button_clicked()
{
// Set the "load safe yaml" button to be disabled
ui->load_dronex_yaml_button->setEnabled(false);
// Send a message requesting the parameters from the YAML
// file to be reloaded for the safe controller
std_msgs::Int32 msg;
msg.data = LOAD_YAML_DRONEX_CONTROLLER_AGENT;
this->requestLoadControllerYamlPublisher.publish(msg);
ROS_INFO("Request load of dronex controller YAML published");
// Start a timer which will enable the button in its callback
// > This is required because the agent node waits some time between
// re-loading the values from the YAML file and then assigning then
// to the local variable of the agent.
// > Thus we use this timer to prevent the user from clicking the
// button in the GUI repeatedly.
ros::NodeHandle nodeHandle("~");
m_timer_yaml_file_for_dronex_controller = nodeHandle.createTimer(ros::Duration(1.5), &MainWindow::dronexYamlFileTimerCallback, this, true);
}
void MainWindow::dronexYamlFileTimerCallback(const ros::TimerEvent&)
{
// Enble the "load safe yaml" button again
ui->load_dronex_yaml_button->setEnabled(true);
}
......@@ -1365,6 +1429,21 @@ void MainWindow::requestLoadControllerYaml_from_my_GUI_Callback(const std_msgs::
// Switch between loading for the different controllers
switch(controller_to_load_yaml)
{
case LOAD_YAML_DRONEX_CONTROLLER_AGENT:
case LOAD_YAML_DRONEX_CONTROLLER_COORDINATOR:
// Set the "load safe yaml" button to be disabled
ui->load_dronex_yaml_button->setEnabled(false);
// Start a timer which will enable the button in its callback
// > This is required because the agent node waits some time between
// re-loading the values from the YAML file and then assigning then
// to the local variable of the agent.
// > Thus we use this timer to prevent the user from clicking the
// button in the GUI repeatedly.
m_timer_yaml_file_for_dronex_controller = nodeHandle.createTimer(ros::Duration(1.5), &MainWindow::dronexYamlFileTimerCallback, this, true);
break;
case LOAD_YAML_SAFE_CONTROLLER_AGENT:
case LOAD_YAML_SAFE_CONTROLLER_COORDINATOR:
// Set the "load safe yaml" button to be disabled
......@@ -1881,6 +1960,9 @@ void MainWindow::on_land_on_dronex_button_clicked(){
send_dronex_button_clicked_message(DRONEX_LAND);
}
void MainWindow::on_abort_dronex_button_clicked(){
send_dronex_button_clicked_message(DRONEX_ABORT);
}
......
pps_ws/src/d_fall_pps/GUI_Qt/studentGUI/src/MainWindow.ui
View file @ 574f6b06
......@@ -409,18 +409,79 @@
<item row="0" column="0">
<layout class="QHBoxLayout" name="horizontalLayout_14">
<item>
<widget class="QPushButton" name="take_off_dronex_button">
<property name="text">
<string>Take off from Mothership</string>
</property>
</widget>
</item>
<item>
<widget class="QPushButton" name="land_on_dronex_button">
<property name="text">
<string>Land on Mothership</string>
</property>
</widget>
<layout class="QVBoxLayout" name="verticalLayout_10">
<item>
<widget class="QPushButton" name="take_off_dronex_button">
<property name="sizePolicy">
<sizepolicy hsizetype="Minimum" vsizetype="Ignored">
<horstretch>0</horstretch>
<verstretch>0</verstretch>
</sizepolicy>
</property>
<property name="font">
<font>
<weight>75</weight>
<bold>true</bold>
</font>
</property>
<property name="text">
<string>Take Off</string>
</property>
</widget>
</item>
<item>
<widget class="QPushButton" name="land_on_dronex_button">
<property name="sizePolicy">
<sizepolicy hsizetype="Minimum" vsizetype="Ignored">
<horstretch>0</horstretch>
<verstretch>0</verstretch>
</sizepolicy>
</property>
<property name="font">
<font>
<weight>75</weight>
<bold>true</bold>
</font>
</property>
<property name="text">
<string>Land on Mothership</string>
</property>
<property name="flat">
<bool>false</bool>
</property>
</widget>
</item>
<item>
<widget class="QPushButton" name="abort_dronex_button">
<property name="sizePolicy">
<sizepolicy hsizetype="Minimum" vsizetype="Ignored">
<horstretch>0</horstretch>
<verstretch>0</verstretch>
</sizepolicy>
</property>
<property name="font">
<font>
<weight>75</weight>
<italic>false</italic>
<bold>true</bold>
<strikeout>false</strikeout>
</font>
</property>
<property name="mouseTracking">
<bool>false</bool>
</property>
<property name="acceptDrops">
<bool>false</bool>
</property>
<property name="text">
<string>Abort Misson</string>
</property>
<property name="flat">
<bool>false</bool>
</property>
</widget>
</item>
</layout>
</item>
</layout>
</item>
......@@ -4035,6 +4096,69 @@
<property name="bottomMargin">
<number>6</number>
</property>
<item row="0" column="2">
<widget class="QLabel" name="label_2">
<property name="sizePolicy">
<sizepolicy hsizetype="Fixed" vsizetype="Fixed">
<horstretch>0</horstretch>
<verstretch>0</verstretch>
</sizepolicy>
</property>
<property name="minimumSize">
<size>
<width>180</width>
<height>30</height>
</size>
</property>
<property name="maximumSize">
<size>
<width>180</width>
<height>30</height>
</size>
</property>
<property name="font">
<font>
<pointsize>16</pointsize>
</font>
</property>
<property name="text">
<string>Load</string>
</property>
<property name="alignment">
<set>Qt::AlignCenter</set>
</property>
</widget>
</item>
<item row="5" column="0">
<widget class="QPushButton" name="enable_demo_controller">
<property name="sizePolicy">
<sizepolicy hsizetype="Fixed" vsizetype="Fixed">
<horstretch>0</horstretch>
<verstretch>0</verstretch>
</sizepolicy>
</property>
<property name="minimumSize">
<size>
<width>180</width>
<height>50</height>
</size>
</property>
<property name="maximumSize">
<size>
<width>180</width>
<height>50</height>
</size>
</property>
<property name="font">
<font>
<pointsize>14</pointsize>
</font>
</property>
<property name="text">
<string>Demo</string>
</property>
</widget>
</item>
<item row="2" column="0">
<widget class="QPushButton" name="enable_droneX_controller">
<property name="sizePolicy">
......@@ -4278,39 +4402,6 @@
</property>
</widget>
</item>
<item row="0" column="2">
<widget class="QLabel" name="label_2">
<property name="sizePolicy">
<sizepolicy hsizetype="Fixed" vsizetype="Fixed">
<horstretch>0</horstretch>
<verstretch>0</verstretch>
</sizepolicy>
</property>
<property name="minimumSize">
<size>
<width>180</width>
<height>30</height>
</size>
</property>
<property name="maximumSize">
<size>
<width>180</width>
<height>30</height>
</size>
</property>
<property name="font">
<font>
<pointsize>16</pointsize>
</font>
</property>
<property name="text">
<string>Load</string>
</property>
<property name="alignment">
<set>Qt::AlignCenter</set>
</property>
</widget>
</item>
<item row="0" column="0">
<widget class="QLabel" name="label_21">
<property name="sizePolicy">
......@@ -4379,36 +4470,6 @@
</property>
</widget>
</item>
<item row="5" column="0">
<widget class="QPushButton" name="enable_demo_controller">
<property name="sizePolicy">
<sizepolicy hsizetype="Fixed" vsizetype="Fixed">
<horstretch>0</horstretch>
<verstretch>0</verstretch>
</sizepolicy>
</property>
<property name="minimumSize">
<size>
<width>180</width>
<height>50</height>
</size>
</property>
<property name="maximumSize">
<size>
<width>180</width>
<height>50</height>
</size>
</property>
<property name="font">
<font>
<pointsize>14</pointsize>
</font>
</property>
<property name="text">
<string>Demo</string>
</property>
</widget>
</item>
<item row="11" column="0">
<spacer name="verticalSpacer">
<property name="orientation">
......@@ -4697,6 +4758,26 @@
</property>
</widget>
</item>
<item row="2" column="2">
<widget class="QPushButton" name="load_dronex_yaml_button">
<property name="sizePolicy">
<sizepolicy hsizetype="Ignored" vsizetype="Ignored">
<horstretch>0</horstretch>
<verstretch>0</verstretch>
</sizepolicy>
</property>
<property name="font">
<font>
<pointsize>14</pointsize>
<weight>50</weight>
<bold>false</bold>
</font>
</property>
<property name="text">
<string>DroneX</string>
</property>
</widget>
</item>
</layout>
</item>
<item>
......
pps_ws/src/d_fall_pps/include/nodes/DroneXControllerService.h
View file @ 574f6b06
......@@ -96,16 +96,21 @@
#define DRONEX_BUTTON_DISCONNECT 7
#define DRONEX_TAKE_OFF 8
#define DRONEX_LAND 9
#define DRONEX_ABORT 10
#define DRONEX_BUTTON_1 11
#define DRONEX_BUTTON_2 12
#define DRONEX_BUTTON_3 13
#define DRONEX_BUTTON_4 14
#define DRONEX_STATE_GROUND 0
#define DRONEX_STATE_TAKING_OFF 1
#define DRONEX_STATE_LANDING 2
#define DRONEX_STATE_APPROACH 3
#define DRONEX_STATE_GROUND 0
#define DRONEX_STATE_TAKING_OFF 1
#define DRONEX_STATE_LAND_ON_GROUND 2
#define DRONEX_STATE_APPROACH 3
#define DRONEX_STATE_HOVER 4
#define DRONEX_STATE_LAND_ON_MOTHERSHIP 5
#define DRONEX_STATE_ON_MOTHERSHIP 6
// These constants define the modes that can be used for controller the Crazyflie 2.0,
......@@ -177,7 +182,7 @@ float m_time_seconds;
float m_mass_CF_grams;
Setpoint dronexSetpoint;
int flying_state = DRONEX_STATE_APPROACH;
int flying_state = DRONEX_STATE_GROUND;
//describes the area of the crazyflie and other parameters
......@@ -378,9 +383,11 @@ void buttonPressed_4();
void buttonPressed_take_off();
void buttonPressed_land();
void buttonPressed_abort();
void motorsOFF(Controller::Response &response);
// CONTROLLER COMPUTATIONS
// > The function that is called to "start" all estimation and control computations
......
pps_ws/src/d_fall_pps/include/nodes/PPSClient.h
View file @ 574f6b06
......@@ -165,8 +165,6 @@ ros::ServiceClient tuningController;
// The Picker controller specified in the ClientConfig.yaml
ros::ServiceClient pickerController;
// The DroneX controller specified in the ClientConfig.yaml
//TODO
ros::ServiceClient droneXController;
......
pps_ws/src/d_fall_pps/include/nodes/ParameterService.h
View file @ 574f6b06
// Copyright (C) 2017, ETH Zurich, D-ITET, Paul Beuchat
//
// This file is part of D-FaLL-System.
//
//
// D-FaLL-System is free software: you can redistribute it and/or modify
// it under the terms of the GNU General Public License as published by
// the Free Software Foundation, either version 3 of the License, or
// (at your option) any later version.
//
//
// D-FaLL-System is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
// GNU General Public License for more details.
//
//
// You should have received a copy of the GNU General Public License
// along with D-FaLL-System. If not, see <http://www.gnu.org/licenses/>.
//
//
//
// ----------------------------------------------------------------------------------
// DDDD FFFFF L L SSSS Y Y SSSS TTTTT EEEEE M M
......
pps_ws/src/d_fall_pps/src/nodes/DroneXControllerService.cpp
View file @ 574f6b06
......@@ -191,17 +191,34 @@ void perControlCycleOperations()
void buttonPressed_take_off(){
ROS_INFO("[DRONEX CONTROLLER-DroneXControllerService] Taking off...");
flying_state = DRONEX_STATE_TAKING_OFF;
if(flying_state == DRONEX_STATE_GROUND || flying_state == DRONEX_STATE_ON_MOTHERSHIP){
ROS_INFO("[DRONEX CONTROLLER-DroneXControllerService] Taking off...");
flying_state = DRONEX_STATE_TAKING_OFF;
}else{
ROS_ERROR("Cannot change to DRONEX_STATE_TAKING_OFF");
}
}
void buttonPressed_land(){
ROS_INFO("[DRONEX CONTROLLER-DroneXControllerService] Landing...");
flying_state = DRONEX_STATE_LANDING;
if(flying_state == DRONEX_STATE_HOVER){
ROS_INFO("[DRONEX CONTROLLER-DroneXControllerService] Landing...");
flying_state = DRONEX_STATE_LAND_ON_MOTHERSHIP;
}else{
ROS_ERROR("Cannot change to DRONEX_STATE_LAND_ON_MOTHERSHIP");
}
// flying_state = DRONEX_STATE_APPROACHING; iff already took off
}
void buttonPressed_abort(){
ROS_INFO("[DRONEX CONTROLLER-DroneXControllerService] Abort Mission!");
flying_state = DRONEX_STATE_LAND_ON_GROUND;
}
......@@ -327,7 +344,7 @@ bool calculateControlOutput(Controller::Request &request, Controller::Response &
dronexSetpoint.y = request.otherCrazyflies[0].y;
dronexSetpoint.z = request.otherCrazyflies[0].z + 0.2;
//setpointCallback(dronexSetpoint);
}
break;
case DRONEX_STATE_GROUND:
......@@ -340,17 +357,17 @@ bool calculateControlOutput(Controller::Request &request, Controller::Response &
}*/
break;
case DRONEX_STATE_LANDING:
case DRONEX_STATE_LAND_ON_GROUND:
{
//ROS_INFO_STREAM("DRONEX_STATE_LANDING: Crazyflie z coordinate " << request.ownCrazyflie.z);
//ROS_INFO_STREAM("DRONEX_STATE_LANDING: Mothership z coordinate " << request.otherCrazyflies[0].z);
//ROS_INFO_STREAM("DRONEX_STATE_LAND_ON_GROUND: Crazyflie z coordinate " << request.ownCrazyflie.z);
//ROS_INFO_STREAM("DRONEX_STATE_LAND_ON_GROUND: Mothership z coordinate " << request.otherCrazyflies[0].z);
dronexSetpoint.x = request.otherCrazyflies[0].x; //set setpoint to droneX x y and z coordinates
dronexSetpoint.y = request.otherCrazyflies[0].y;
dronexSetpoint.z = request.otherCrazyflies[0].z+0.05;
/*if(request.ownCrazyflie.z < request.otherCrazyflies[0].z+0.25){ // >0.1
std_msgs::Int32 flying_state_msg;
flying_state_msg.data = STATE_MOTORS_OFF;
......@@ -359,14 +376,14 @@ bool calculateControlOutput(Controller::Request &request, Controller::Response &
ROS_INFO_STREAM("DRONEX_MOTORS_OFF..." << request.ownCrazyflie.z - request.otherCrazyflies[0].z);
return true;
}*/
/*
/*
}else{
//if(dronexSetpoint.z>=request.otherCrazyflies[0].z){
float down_per_cycle = 0.003;
if(dronexSetpoint.z < request.otherCrazyflies[0].z+0.5){
down_per_cycle = 0.0005;
}
dronexSetpoint.z -= down_per_cycle;
ROS_INFO_STREAM("DRONEX_L: z" << dronexSetpoint.z);
//}else{
......@@ -374,8 +391,8 @@ bool calculateControlOutput(Controller::Request &request, Controller::Response &
//}
}*/
}
break;
......@@ -385,7 +402,7 @@ bool calculateControlOutput(Controller::Request &request, Controller::Response &
dronexSetpoint.x = request.otherCrazyflies[0].x;
dronexSetpoint.y = request.otherCrazyflies[0].y;