diff --git a/dfall_ws/src/dfall_pkg/include/nodes/DefaultControllerService.h b/dfall_ws/src/dfall_pkg/include/nodes/DefaultControllerService.h
index 90c53afc25bc735dc1370efa545a148c53c87a4d..fb83bc8ea77490f845fc0f5da5abe7ba593461fa 100644
--- a/dfall_ws/src/dfall_pkg/include/nodes/DefaultControllerService.h
+++ b/dfall_ws/src/dfall_pkg/include/nodes/DefaultControllerService.h
@@ -70,6 +70,7 @@
#include "dfall_pkg/IntIntService.h"
#include "dfall_pkg/LoadYamlFromFilename.h"
#include "dfall_pkg/GetSetpointService.h"
+#include "dfall_pkg/CMQuery.h"
// Include the shared definitions
#include "nodes/Constants.h"
@@ -377,7 +378,19 @@ ros::Publisher m_manoeuvreCompletePublisher;
// to the flying agent client
ros::Publisher m_motorsOffToFlyingAgentClientPublisher;
+// Describes the area of the crazyflie and other parameters
+//CrazyflieContext m_context;
+// Flag for whether a context is available or not
+bool m_isAvailableContext = false;
+
+// Variable for each coordinate
+float m_symmetric_area_bounds_x = 0.5;
+float m_symmetric_area_bounds_y = 0.5;
+float m_symmetric_area_bounds_z = 0.5;
+
+// Service Client from which the "CrazyflieContext" is loaded
+ros::ServiceClient m_centralManager;
@@ -461,12 +474,21 @@ bool getCurrentSetpointCallback(GetSetpointService::Request &request, GetSetpoin
// PUBLISH THE CURRENT SETPOINT AND STATE
void publishCurrentSetpointAndState();
+// CLIP TO MIN AND MAX
+float clipToBounds(float val, float val_min, float val_max);
+
// CUSTOM COMMAND RECEIVED CALLBACK
void customCommandReceivedCallback(const CustomButtonWithHeader& commandReceived);
// PUBLISH MOTORS-OFF MESSAGE TO FLYING AGENT CLIENT
void publish_motors_off_to_flying_agent_client();
+// FUNCTIONS FOR THE CONTEXT OF THIS AGENT
+// > Callback that the context database changed
+void crazyflieContextDatabaseChangedCallback(const std_msgs::Int32& msg);
+// > For loading the context of this agent
+void loadCrazyflieContext();
+
// LOADING OF YAML PARAMETERS
void timerCallback_initial_load_yaml(const ros::TimerEvent&);
void isReadyDefaultControllerYamlCallback(const IntWithHeader & msg);
diff --git a/dfall_ws/src/dfall_pkg/param/DefaultController.yaml b/dfall_ws/src/dfall_pkg/param/DefaultController.yaml
index 812ce053633675aaf27fae11cfbf602d530f7eab..bb232502ca80158afeb3f4e4f93ce53e4a26a744 100644
--- a/dfall_ws/src/dfall_pkg/param/DefaultController.yaml
+++ b/dfall_ws/src/dfall_pkg/param/DefaultController.yaml
@@ -99,8 +99,8 @@ gainMatrixThrust_2StateVector : [ 0.98, 0.25]
gainMatrixRollRate_3StateVector : [-6.20,-3.00, 5.20]
gainMatrixPitchRate_3StateVector : [ 6.20, 3.00, 5.20]
# The LQR Controller parameters for mode 2 (Angle-nested)
-gainMatrixRollAngle_2StateVector : [-0.50,-0.30]
-gainMatrixPitchAngle_2StateVector : [ 0.50, 0.30]
+gainMatrixRollAngle_2StateVector : [-0.80,-0.50]
+gainMatrixPitchAngle_2StateVector : [ 0.80, 0.50]
gainRollRate_fromAngle : 4.00
gainPitchRate_fromAngle : 4.00
# The LQR Controller parameters for yaw
diff --git a/dfall_ws/src/dfall_pkg/src/nodes/DefaultControllerService.cpp b/dfall_ws/src/dfall_pkg/src/nodes/DefaultControllerService.cpp
index 8fd187147f0b40e59aa0d7bdfbd93e97aeb84360..f05ce8008c6307398f7fd1948887ab1c6558c465 100644
--- a/dfall_ws/src/dfall_pkg/src/nodes/DefaultControllerService.cpp
+++ b/dfall_ws/src/dfall_pkg/src/nodes/DefaultControllerService.cpp
@@ -1310,11 +1310,22 @@ void requestSetpointChangeCallback(const SetpointWithHeader& newSetpoint)
// CHANGE SETPOINT FUNCTION
void setNewSetpoint(float x, float y, float z, float yaw)
{
- // Put the new setpoint into the class variable
- m_setpoint[0] = x;
- m_setpoint[1] = y;
- m_setpoint[2] = z;
- m_setpoint[3] = yaw;
+ if (!m_isAvailableContext)
+ {
+ ROS_ERROR("[DEFAULT CONTROLLER] New setpoint requested, however there is no context available, setting default setpoint instead.");
+ m_setpoint[0] = yaml_default_setpoint[0];
+ m_setpoint[1] = yaml_default_setpoint[1];
+ m_setpoint[2] = yaml_default_setpoint[2];
+ m_setpoint[3] = yaml_default_setpoint[3];
+ }
+ else
+ {
+ // Put the new setpoint into the class variable
+ m_setpoint[0] = clipToBounds( x , -m_symmetric_area_bounds_x , m_symmetric_area_bounds_x );
+ m_setpoint[1] = clipToBounds( y , -m_symmetric_area_bounds_y , m_symmetric_area_bounds_y );;
+ m_setpoint[2] = clipToBounds( z , -m_symmetric_area_bounds_z , m_symmetric_area_bounds_z );;
+ m_setpoint[3] = yaw;
+ }
// Publish the change so that the network is updated
// (mainly the "flying agent GUI" is interested in
@@ -1323,10 +1334,10 @@ void setNewSetpoint(float x, float y, float z, float yaw)
// Instantiate a local variable of type "SetpointWithHeader"
SetpointWithHeader msg;
// Fill in the setpoint
- msg.x = x;
- msg.y = y;
- msg.z = z;
- msg.yaw = yaw;
+ msg.x = m_setpoint[0];
+ msg.y = m_setpoint[1];
+ msg.z = m_setpoint[2];
+ msg.yaw = m_setpoint[3];
// Publish the message
m_setpointChangedPublisher.publish(msg);
}
@@ -1363,6 +1374,16 @@ void publishCurrentSetpointAndState()
m_setpointChangedPublisher.publish(msg);
}
+float clipToBounds(float val, float val_min, float val_max)
+{
+ float return_val = val;
+ if (return_val < val_min)
+ return_val = val_min;
+ if (return_val > val_max)
+ return_val = val_max;
+
+ return return_val;
+}
@@ -1464,6 +1485,79 @@ void publish_motors_off_to_flying_agent_client()
+// ----------------------------------------------------------------------------------
+// L OOO A DDDD
+// L O O A A D D
+// L O O A A D D
+// L O O AAAAA D D
+// LLLLL OOO A A DDDD
+//
+// CCCC OOO N N TTTTT EEEEE X X TTTTT
+// C O O NN N T E X X T
+// C O O N N N T EEE X T
+// C O O N NN T E X X T
+// CCCC OOO N N T EEEEE X X T
+// ----------------------------------------------------------------------------------
+
+
+void crazyflieContextDatabaseChangedCallback(const std_msgs::Int32& msg)
+{
+ ROS_INFO("[DEFAULT CONTROLLER] Received message that the Context Database Changed");
+ loadCrazyflieContext();
+}
+
+
+
+void loadCrazyflieContext()
+{
+ CMQuery contextCall;
+ contextCall.request.studentID = m_agentID;
+ ROS_INFO_STREAM("[DEFAULT CONTROLLER] AgentID:" << m_agentID);
+
+ CrazyflieContext new_context;
+
+ m_centralManager.waitForExistence(ros::Duration(-1));
+
+ if(m_centralManager.call(contextCall)) {
+ new_context = contextCall.response.crazyflieContext;
+ //ROS_INFO_STREAM("[DEFAULT CONTROLLER] CrazyflieContext:\n" << new_context);
+
+ //if((m_context.crazyflieName != "") && (new_context.crazyflieName != m_context.crazyflieName)) //linked crazyflie name changed and it was not empty before
+ //{
+
+ // Motors off is done in python script now everytime we disconnect
+
+ // send motors OFF and disconnect before setting m_context = new_context
+ // std_msgs::Int32 msg;
+ // msg.data = CMD_CRAZYFLY_MOTORS_OFF;
+ // commandPublisher.publish(msg);
+
+ // ROS_INFO("[DEFAULT CONTROLLER] CF is now different for this student. Disconnect and turn it off");
+
+ // IntWithHeader msg;
+ // msg.shouldCheckForAgentID = false;
+ // msg.data = CMD_DISCONNECT;
+ // m_crazyRadioCommandPublisher.publish(msg);
+ //}
+
+ m_symmetric_area_bounds_x = 0.5 * (new_context.localArea.xmax - new_context.localArea.xmin);
+ m_symmetric_area_bounds_y = 0.5 * (new_context.localArea.ymax - new_context.localArea.ymin);
+ m_symmetric_area_bounds_z = 0.5 * (new_context.localArea.zmax - new_context.localArea.zmin);
+ m_isAvailableContext = true;
+
+ }
+ else
+ {
+ m_isAvailableContext = false;
+ ROS_ERROR("[DEFAULT CONTROLLER] Failed to load context. Waiting for next Save in DB by teacher");
+ }
+}
+
+
+
+
+
+
// ----------------------------------------------------------------------------------
// L OOO A DDDD
@@ -1948,6 +2042,22 @@ int main(int argc, char* argv[])
+ // CREATE A NODE HANDLE TO THE ROOT OF THE D-FaLL SYSTEM
+ ros::NodeHandle nodeHandle_dfall_root("/dfall");
+
+ // LOADING OF THIS AGENT'S CONTEXT
+ // Service cleint for loading the allocated flying
+ // zone and other context details
+ //ros::service::waitForService("/CentralManagerService/CentralManager");
+ m_centralManager = nodeHandle_dfall_root.serviceClient<CMQuery>("CentralManagerService/Query", false);
+ // Call the class function that uses this service
+ // client to load the context
+ loadCrazyflieContext();
+ // Subscriber for when the Flying Zone Database changed
+ ros::Subscriber databaseChangedSubscriber = nodeHandle_dfall_root.subscribe("CentralManagerService/DBChanged", 1, crazyflieContextDatabaseChangedCallback);
+
+
+
// Print out some information to the user.
ROS_INFO("[DEFAULT CONTROLLER] Service ready :-)");
diff --git a/dfall_ws/src/dfall_pkg/src/nodes/FlyingAgentClient.cpp b/dfall_ws/src/dfall_pkg/src/nodes/FlyingAgentClient.cpp
index eb882d0bbabdbe9c409888a052eebc3df8b7b547..210946f021a8df09d9eac05a21d7bcef591b4edc 100755
--- a/dfall_ws/src/dfall_pkg/src/nodes/FlyingAgentClient.cpp
+++ b/dfall_ws/src/dfall_pkg/src/nodes/FlyingAgentClient.cpp
@@ -391,13 +391,15 @@ void sendZeroOutputCommandForMotors()
bool safetyCheck_on_positionAndTilt(CrazyflieData data)
{
// Check on the X position
- if((data.x < m_context.localArea.xmin) or (data.x > m_context.localArea.xmax))
+ float symmetric_bound_x = 0.5 * (m_context.localArea.xmax-m_context.localArea.xmin);
+ if((data.x < -symmetric_bound_x) or (data.x > symmetric_bound_x))
{
ROS_INFO_STREAM("[FLYING AGENT CLIENT] x safety failed");
return false;
}
// Check on the Y position
- if((data.y < m_context.localArea.ymin) or (data.y > m_context.localArea.ymax))
+ float symmetric_bound_y = 0.5 * (m_context.localArea.ymax-m_context.localArea.ymin);
+ if((data.y < -symmetric_bound_y) or (data.y > symmetric_bound_y))
{
ROS_INFO_STREAM("[FLYING AGENT CLIENT] y safety failed");
return false;