Commit a6f0c42a authored by beuchatp's avatar beuchatp
Browse files

Started process of converting the Flying agent client from the safe controller...

Started process of converting the Flying agent client from the safe controller to the default controller, and streamlining things at the same time. Currently compiles and flys, but needs cleaning up and proper implementing of the take-off and land functions.
parent ac1cb5ab
......@@ -107,7 +107,7 @@
#define CF_COMMAND_TYPE_ANGLE 8
// Types of controllers being used:
#define SAFE_CONTROLLER 1
#define DEFAULT_CONTROLLER 1
#define DEMO_CONTROLLER 2
#define STUDENT_CONTROLLER 3
#define MPC_CONTROLLER 4
......@@ -118,7 +118,7 @@
// The constants that "command" changes in the
// operation state of this agent
#define CMD_USE_SAFE_CONTROLLER 1
#define CMD_USE_DEFAULT_CONTROLLER 1
#define CMD_USE_DEMO_CONTROLLER 2
#define CMD_USE_STUDENT_CONTROLLER 3
#define CMD_USE_MPC_CONTROLLER 4
......@@ -208,84 +208,4 @@
// ----------------------------------------------------------------------------------
// For standard buttons in the GUI
#define REQUEST_DEFAULT_SETPOINT_BUTTON_ID 100
// OLD STUFF FOR LOADING YAML FILES
// For which controller parameters to load from file
#define LOAD_YAML_SAFE_CONTROLLER_AGENT 1
#define LOAD_YAML_DEMO_CONTROLLER_AGENT 2
#define LOAD_YAML_STUDENT_CONTROLLER_AGENT 3
#define LOAD_YAML_MPC_CONTROLLER_AGENT 4
#define LOAD_YAML_REMOTE_CONTROLLER_AGENT 5
#define LOAD_YAML_TUNING_CONTROLLER_AGENT 6
#define LOAD_YAML_PICKER_CONTROLLER_AGENT 7
#define LOAD_YAML_TEMPLATE_CONTROLLER_AGENT 8
#define LOAD_YAML_SAFE_CONTROLLER_COORDINATOR 11
#define LOAD_YAML_DEMO_CONTROLLER_COORDINATOR 12
#define LOAD_YAML_STUDENT_CONTROLLER_COORDINATOR 13
#define LOAD_YAML_MPC_CONTROLLER_COORDINATOR 14
#define LOAD_YAML_REMOTE_CONTROLLER_COORDINATOR 15
#define LOAD_YAML_TUNING_CONTROLLER_COORDINATOR 16
#define LOAD_YAML_PICKER_CONTROLLER_COORDINATOR 17
#define LOAD_YAML_TEMPLATE_CONTROLLER_COORDINATOR 18
// For sending commands to the controller node informing
// which parameters to fetch
// > NOTE: these are identical to the #defines above, but
// used because they have the name distinguishes
// between:
// - "loading" a yaml file into ram
// - "fetching" the values that were loaded into ram
#define FETCH_YAML_SAFE_CONTROLLER_FROM_OWN_AGENT 1
#define FETCH_YAML_DEMO_CONTROLLER_FROM_OWN_AGENT 2
#define FETCH_YAML_STUDENT_CONTROLLER_FROM_OWN_AGENT 3
#define FETCH_YAML_MPC_CONTROLLER_FROM_OWN_AGENT 4
#define FETCH_YAML_REMOTE_CONTROLLER_FROM_OWN_AGENT 5
#define FETCH_YAML_TUNING_CONTROLLER_FROM_OWN_AGENT 6
#define FETCH_YAML_PICKER_CONTROLLER_FROM_OWN_AGENT 7
#define FETCH_YAML_TEMPLATE_CONTROLLER_FROM_OWN_AGENT 8
#define FETCH_YAML_SAFE_CONTROLLER_FROM_COORDINATOR 11
#define FETCH_YAML_DEMO_CONTROLLER_FROM_COORDINATOR 12
#define FETCH_YAML_STUDENT_CONTROLLER_FROM_COORDINATOR 13
#define FETCH_YAML_MPC_CONTROLLER_FROM_COORDINATOR 14
#define FETCH_YAML_REMOTE_CONTROLLER_FROM_COORDINATOR 15
#define FETCH_YAML_TUNING_CONTROLLER_FROM_COORDINATOR 16
#define FETCH_YAML_TEMPLATE_CONTROLLER_FROM_COORDINATOR 18
#define REQUEST_DEFAULT_SETPOINT_BUTTON_ID 100
\ No newline at end of file
......@@ -72,17 +72,7 @@
#endif
// COMMANDS FOR THE FLYING STATE/CONTROLLER USED
// The constants that "command" changes in the
// operation state of this agent. These "commands"
// are sent from this GUI node to the "FlyingAgentClient"
// node where the command is enacted
// #define CMD_USE_SAFE_CONTROLLER 1
// #define CMD_USE_DEMO_CONTROLLER 2
// #define CMD_USE_STUDENT_CONTROLLER 3
// #define CMD_USE_MPC_CONTROLLER 4
// #define CMD_USE_REMOTE_CONTROLLER 5
// #define CMD_USE_TUNING_CONTROLLER 6
namespace Ui {
......
......@@ -397,16 +397,11 @@ void ControllerTabs::setControllerEnabled(int new_controller)
// the enable controller
switch(new_controller)
{
case SAFE_CONTROLLER:
{
setTextColourOfTabLabel( m_tab_text_colour_highlight , ui->default_tab );
break;
}
// case DEFAULT_CONTROLLER:
// {
// setTextColourOfTabLabel( m_tab_text_colour_highlight , ui->default_tab );
// break;
// }
case DEFAULT_CONTROLLER:
{
setTextColourOfTabLabel( m_tab_text_colour_highlight , ui->default_tab );
break;
}
case DEMO_CONTROLLER:
{
//ui->controller_enabled_label->setText("Demo");
......
......@@ -88,7 +88,7 @@ CoordinatorRow::CoordinatorRow(QWidget *parent, int agentID) :
setFlyingState(STATE_UNAVAILABLE);
// SET THE DEFAULT NAME FOR THE SELECTED CONTROLLER
setControllerEnabled(SAFE_CONTROLLER);
setControllerEnabled(DEFAULT_CONTROLLER);
#ifdef CATKIN_MAKE
......@@ -708,9 +708,9 @@ void CoordinatorRow::setControllerEnabled(int new_controller)
{
switch(new_controller)
{
case SAFE_CONTROLLER:
case DEFAULT_CONTROLLER:
{
ui->controller_enabled_label->setText("Safe");
ui->controller_enabled_label->setText("Default");
break;
}
case DEMO_CONTROLLER:
......
......@@ -125,9 +125,9 @@ void EnableControllerLoadYamlBar::on_enable_default_button_clicked()
#ifdef CATKIN_MAKE
dfall_pkg::IntWithHeader msg;
fillIntMessageHeader(msg);
msg.data = CMD_USE_SAFE_CONTROLLER;
msg.data = CMD_USE_DEFAULT_CONTROLLER;
this->commandPublisher.publish(msg);
ROS_INFO("[ENABLE CONTROLLER LOAD YAML GUI BAR] Enable Safe Controller");
ROS_INFO("[ENABLE CONTROLLER LOAD YAML GUI BAR] Enable Default Controller");
#endif
}
......
......@@ -107,7 +107,7 @@
#define CF_COMMAND_TYPE_ANGLE 8
// Types of controllers being used:
#define SAFE_CONTROLLER 1
#define DEFAULT_CONTROLLER 1
#define DEMO_CONTROLLER 2
#define STUDENT_CONTROLLER 3
#define MPC_CONTROLLER 4
......@@ -117,7 +117,7 @@
// The constants that "command" changes in the
// operation state of this agent
#define CMD_USE_SAFE_CONTROLLER 1
#define CMD_USE_DEFAULT_CONTROLLER 1
#define CMD_USE_DEMO_CONTROLLER 2
#define CMD_USE_STUDENT_CONTROLLER 3
#define CMD_USE_MPC_CONTROLLER 4
......@@ -206,81 +206,4 @@
// ----------------------------------------------------------------------------------
// For standard buttons in the GUI
#define REQUEST_DEFAULT_SETPOINT_BUTTON_ID 100
// OLD STUFF FOR LOADING YAML FILES
// For which controller parameters to load from file
#define LOAD_YAML_SAFE_CONTROLLER_AGENT 1
#define LOAD_YAML_DEMO_CONTROLLER_AGENT 2
#define LOAD_YAML_STUDENT_CONTROLLER_AGENT 3
#define LOAD_YAML_MPC_CONTROLLER_AGENT 4
#define LOAD_YAML_REMOTE_CONTROLLER_AGENT 5
#define LOAD_YAML_TUNING_CONTROLLER_AGENT 6
#define LOAD_YAML_PICKER_CONTROLLER_AGENT 7
#define LOAD_YAML_SAFE_CONTROLLER_COORDINATOR 11
#define LOAD_YAML_DEMO_CONTROLLER_COORDINATOR 12
#define LOAD_YAML_STUDENT_CONTROLLER_COORDINATOR 13
#define LOAD_YAML_MPC_CONTROLLER_COORDINATOR 14
#define LOAD_YAML_REMOTE_CONTROLLER_COORDINATOR 15
#define LOAD_YAML_TUNING_CONTROLLER_COORDINATOR 16
#define LOAD_YAML_PICKER_CONTROLLER_COORDINATOR 17
// For sending commands to the controller node informing
// which parameters to fetch
// > NOTE: these are identical to the #defines above, but
// used because they have the name distinguishes
// between:
// - "loading" a yaml file into ram
// - "fetching" the values that were loaded into ram
#define FETCH_YAML_SAFE_CONTROLLER_FROM_OWN_AGENT 1
#define FETCH_YAML_DEMO_CONTROLLER_FROM_OWN_AGENT 2
#define FETCH_YAML_STUDENT_CONTROLLER_FROM_OWN_AGENT 3
#define FETCH_YAML_MPC_CONTROLLER_FROM_OWN_AGENT 4
#define FETCH_YAML_REMOTE_CONTROLLER_FROM_OWN_AGENT 5
#define FETCH_YAML_TUNING_CONTROLLER_FROM_OWN_AGENT 6
#define FETCH_YAML_PICKER_CONTROLLER_FROM_OWN_AGENT 7
#define FETCH_YAML_SAFE_CONTROLLER_FROM_COORDINATOR 11
#define FETCH_YAML_DEMO_CONTROLLER_FROM_COORDINATOR 12
#define FETCH_YAML_STUDENT_CONTROLLER_FROM_COORDINATOR 13
#define FETCH_YAML_MPC_CONTROLLER_FROM_COORDINATOR 14
#define FETCH_YAML_REMOTE_CONTROLLER_FROM_COORDINATOR 15
#define FETCH_YAML_TUNING_CONTROLLER_FROM_COORDINATOR 16
#define FETCH_YAML_PICKER_CONTROLLER_FROM_COORDINATOR 17
\ No newline at end of file
#define REQUEST_DEFAULT_SETPOINT_BUTTON_ID 100
\ No newline at end of file
......@@ -54,12 +54,6 @@ DISCONNECTED = 2
CMD_RECONNECT = 0
CMD_DISCONNECT = 1
# Commands for FlyingAgentClient
CMD_USE_SAFE_CONTROLLER = 1
CMD_USE_CUSTOM_CONTROLLER = 2
CMD_CRAZYFLY_TAKE_OFF = 3
CMD_CRAZYFLY_LAND = 4
CMD_CRAZYFLY_MOTORS_OFF = 5
# rp = RosPack()
# record_file = rp.get_path('dfall_pkg') + '/LoggingOnboard.bag'
......@@ -239,8 +233,6 @@ if __name__ == '__main__':
# rospy.loginfo("Turning off crazyflie")
# change state to motors OFF
# cf_client.FlyingAgentClient_command_pub.publish(CMD_CRAZYFLY_MOTORS_OFF)
#wait for client to send its commands
# time.sleep(1.0)
......
......@@ -120,7 +120,7 @@
// ----------------------------------------------------------------------------------
// Types of controllers being used:
#define SAFE_CONTROLLER 1
#define DEFAULT_CONTROLLER 1
#define DEMO_CONTROLLER 2
#define STUDENT_CONTROLLER 3
#define MPC_CONTROLLER 4
......@@ -131,7 +131,7 @@
// The constants that "command" changes in the
// operation state of this agent
#define CMD_USE_SAFE_CONTROLLER 1
#define CMD_USE_DEFAULT_CONTROLLER 1
#define CMD_USE_DEMO_CONTROLLER 2
#define CMD_USE_STUDENT_CONTROLLER 3
#define CMD_USE_MPC_CONTROLLER 4
......@@ -222,85 +222,4 @@
// ----------------------------------------------------------------------------------
// For standard buttons in the GUI
#define REQUEST_DEFAULT_SETPOINT_BUTTON_ID 100
// OLD STUFF FOR LOADING YAML FILES
// For which controller parameters to load from file
#define LOAD_YAML_SAFE_CONTROLLER_AGENT 1
#define LOAD_YAML_DEMO_CONTROLLER_AGENT 2
#define LOAD_YAML_STUDENT_CONTROLLER_AGENT 3
#define LOAD_YAML_MPC_CONTROLLER_AGENT 4
#define LOAD_YAML_REMOTE_CONTROLLER_AGENT 5
#define LOAD_YAML_TUNING_CONTROLLER_AGENT 6
#define LOAD_YAML_PICKER_CONTROLLER_AGENT 7
#define LOAD_YAML_TEMPLATE_CONTROLLER_AGENT 7
#define LOAD_YAML_SAFE_CONTROLLER_COORDINATOR 11
#define LOAD_YAML_DEMO_CONTROLLER_COORDINATOR 12
#define LOAD_YAML_STUDENT_CONTROLLER_COORDINATOR 13
#define LOAD_YAML_MPC_CONTROLLER_COORDINATOR 14
#define LOAD_YAML_REMOTE_CONTROLLER_COORDINATOR 15
#define LOAD_YAML_TUNING_CONTROLLER_COORDINATOR 16
#define LOAD_YAML_PICKER_CONTROLLER_COORDINATOR 17
#define LOAD_YAML_TEMPLATE_CONTROLLER_COORDINATOR 18
// For sending commands to the controller node informing
// which parameters to fetch
// > NOTE: these are identical to the #defines above, but
// used because they have the name distinguishes
// between:
// - "loading" a yaml file into ram
// - "fetching" the values that were loaded into ram
#define FETCH_YAML_SAFE_CONTROLLER_FROM_OWN_AGENT 1
#define FETCH_YAML_DEMO_CONTROLLER_FROM_OWN_AGENT 2
#define FETCH_YAML_STUDENT_CONTROLLER_FROM_OWN_AGENT 3
#define FETCH_YAML_MPC_CONTROLLER_FROM_OWN_AGENT 4
#define FETCH_YAML_REMOTE_CONTROLLER_FROM_OWN_AGENT 5
#define FETCH_YAML_TUNING_CONTROLLER_FROM_OWN_AGENT 6
#define FETCH_YAML_PICKER_CONTROLLER_FROM_OWN_AGENT 7
#define FETCH_YAML_TEMPLATE_CONTROLLER_FROM_OWN_AGENT 8
#define FETCH_YAML_SAFE_CONTROLLER_FROM_COORDINATOR 11
#define FETCH_YAML_DEMO_CONTROLLER_FROM_COORDINATOR 12
#define FETCH_YAML_STUDENT_CONTROLLER_FROM_COORDINATOR 13
#define FETCH_YAML_MPC_CONTROLLER_FROM_COORDINATOR 14
#define FETCH_YAML_REMOTE_CONTROLLER_FROM_COORDINATOR 15
#define FETCH_YAML_TUNING_CONTROLLER_FROM_COORDINATOR 16
#define FETCH_YAML_PICKER_CONTROLLER_FROM_COORDINATOR 17
#define FETCH_YAML_TEMPLATE_CONTROLLER_FROM_COORDINATOR 18
\ No newline at end of file
#define REQUEST_DEFAULT_SETPOINT_BUTTON_ID 100
\ No newline at end of file
......@@ -121,8 +121,38 @@ std::string m_namespace_to_own_agent_parameter_service;
std::string m_namespace_to_coordinator_parameter_service;
// variables for the states:
int m_flying_state;
bool m_changed_flying_state_flag;
// variable for crazyradio status
int crazyradio_status;
//describes the area of the crazyflie and other parameters
CrazyflieContext m_context;
// The index for which element in the "motion captate data"
// array is expected to match the name in "m_context"
// > Negative numbers indicate unknown
int m_poseDataIndex = -1;
// wheter to use safe of demo controller
// variable for the instant controller, e.g., we use
// safe controller for taking off and landing even
// if demo controller is enabled. This variable WILL change automatically
int m_instant_controller;
ros::ServiceClient* m_instant_controller_service_client;
bool m_controllers_avialble = false;
ros::Timer timer_for_loadAllControllers;
// controller used:
int m_controller_nominally_selected;
// The safe controller specified in the ClientConfig.yaml
ros::ServiceClient safeController;
// The default controller specified in the ClientConfig.yaml
ros::ServiceClient defaultController;
// The Demo controller specified in the ClientConfig.yaml
ros::ServiceClient demoController;
// The Student controller specified in the ClientConfig.yaml
......@@ -139,92 +169,62 @@ ros::ServiceClient pickerController;
ros::ServiceClient templateController;
//values for safteyCheck
bool yaml_strictSafety;
// The values for the safety check on tilt angle
bool yaml_isEnabled_strictSafety = true;
float yaml_angleMargin;
float yaml_maxTiltAngle_for_strictSafety_degrees = 70;
float m_maxTiltAngle_for_strictSafety_redians = 70 * DEG2RAD;
// battery threshold
//float m_battery_threshold_while_flying;
//float m_battery_threshold_while_motors_off;
// battery values
//Setpoint controller_setpoint;
//int m_battery_state;
//float m_battery_voltage;
Setpoint controller_setpoint;
std::vector<float> yaml_default_setpoint = {0.0f, 0.0f, 0.0f, 0.0f};
//std::vector<float> yaml_default_setpoint = {0.0f, 0.0f, 0.0f, 0.0f};
// variables for linear trayectory
Setpoint current_safe_setpoint;
double distance;
double unit_vector[3];
bool was_in_threshold = false;
double yaml_distance_threshold; //to be loaded from yaml
//Setpoint current_safe_setpoint;
//double distance;
//double unit_vector[3];
//bool was_in_threshold = false;
//double yaml_distance_threshold; //to be loaded from yaml
// Service Client from which the "CrazyflieContext" is loaded
ros::ServiceClient centralManager;
ros::Publisher controlCommandPublisher;
// Publisher for the control actions to be sent on
// to the Crazyflie (the CrazyRadio node listen to this
// publisher and actually send the commands)
// {onboardControllerType,roll,pitch,yaw,motorCmd1,motorCmd2,motorCmd3,motorCmd4}
ros::Publisher commandForSendingToCrazyfliePublisher;
// communicate with safeControllerService, setpoint, etc...
ros::Publisher safeControllerServiceSetpointPublisher;
//ros::Publisher safeControllerServiceSetpointPublisher;
// publisher for flying state
// Publisher for the current flying state of this Flying Agent Client
ros::Publisher flyingStatePublisher;
// publisher for battery state
ros::Publisher batteryStatePublisher;
// publisher to send commands to itself.
ros::Publisher commandPublisher;
// Publisher for the commands of:
// {take-off,land,motors-off, and which controller to use}
//ros::Publisher commandPublisher;
// communication with crazyRadio node. Connect and disconnect
// Publisher Communication with crazyRadio node. Connect and disconnect
ros::Publisher crazyRadioCommandPublisher;
// Variable for the namespaces for the paramter services
// > For the paramter service of this agent
std::string namespace_to_own_agent_parameter_service;
// > For the parameter service of the coordinator
std::string namespace_to_coordinator_parameter_service;
// variables for the states:
int flying_state;
bool changed_state_flag;
// variable for crazyradio status
int crazyradio_status;
//describes the area of the crazyflie and other parameters
CrazyflieContext context;
//wheter to use safe of demo controller
int instant_controller; //variable for the instant controller, e.g., we use safe controller for taking off and landing even if demo controller is enabled. This variable WILL change automatically
// controller used:
int controller_used;
// Publisher for which controller is currently being used
ros::Publisher controllerUsedPublisher;
float yaml_take_off_distance;
float yaml_landing_distance;
float yaml_duration_take_off;
float yaml_duration_landing;
// float yaml_take_off_distance;
// float yaml_landing_distance;
// float yaml_duration_take_off;
// float yaml_duration_landing;
bool finished_take_off = false;
bool finished_land = false;
// bool finished_take_off = false;
// bool finished_land = false;
ros::Timer timer_takeoff;
ros::Timer timer_land;
......@@ -250,17 +250,7 @@ ros::Timer timer_land;
// ----------------------------------------------------------------------------------
// > For the LOAD PARAMETERS
//void yamlReadyForFetchCallback(const std_msgs::Int32& msg);
//void fetchYamlParametersForSafeController(ros::NodeHandle& nodeHandle);
//void fetchClientConfigParameters(ros::NodeHandle& nodeHandle);
// > For the LOADING of YAML PARAMETERS
void isReadySafeControllerYamlCallback(const IntWithHeader & msg);
void fetchSafeControllerYamlParameters(ros::NodeHandle& nodeHandle);
void isReadyClientConfigYamlCallback(const IntWithHeader & msg);
void fetchClientConfigYamlParameters(ros::NodeHandle& nodeHandle);
......@@ -268,10 +258,16 @@ void fetchClientConfigYamlParameters(ros::NodeHandle& nodeHandle);
void viconCallback(const ViconData& viconData);
int getPoseDataForObjectNameWithExpectedIndex(const ViconData& viconData, std::string name , int expected_index , CrazyflieData& pose);
void coordinatesToLocal(CrazyflieData& cf);
// > For the SAFETY CHECK on area and the angle
bool safetyCheck(CrazyflieData data, ControlCommand controlCommand);
void changeFlyingStateTo(int new_state);
void crazyflieContextDatabaseChangedCallback(const std_msgs::Int32& msg);
......@@ -279,34 +275,51 @@ void crazyflieContextDatabaseChangedCallback(const std_msgs::Int32& msg);