diff --git a/dfall_ws/src/dfall_pkg/include/nodes/DefaultControllerService.h b/dfall_ws/src/dfall_pkg/include/nodes/DefaultControllerService.h
index fb83bc8ea77490f845fc0f5da5abe7ba593461fa..0bd861de6bfa058756c7cf8a8c5b7ab9f134eebf 100644
--- a/dfall_ws/src/dfall_pkg/include/nodes/DefaultControllerService.h
+++ b/dfall_ws/src/dfall_pkg/include/nodes/DefaultControllerService.h
@@ -314,8 +314,8 @@ std::vector<float> yaml_gainMatrixThrust_2StateVector = { 0.98, 0.25};
std::vector<float> yaml_gainMatrixRollRate_3StateVector = {-6.20,-3.00, 5.20};
std::vector<float> yaml_gainMatrixPitchRate_3StateVector = { 6.20, 3.00, 5.20};
// The LQR Controller parameters for "CONTROLLER_METHOD_RATE_ANGLE_NESTED"
-std::vector<float> yaml_gainMatrixRollAngle_2StateVector = {-0.20,-0.20};
-std::vector<float> yaml_gainMatrixPitchAngle_2StateVector = { 0.20, 0.20};
+std::vector<float> yaml_gainMatrixRollAngle_2StateVector = {-0.80,-0.50};
+std::vector<float> yaml_gainMatrixPitchAngle_2StateVector = { 0.80, 0.50};
float yaml_gainRollRate_fromAngle = 4.00;
float yaml_gainPitchRate_fromAngle = 4.00;
// The LQR Controller parameters for yaw
diff --git a/dfall_ws/src/dfall_pkg/include/nodes/PickerControllerService.h b/dfall_ws/src/dfall_pkg/include/nodes/PickerControllerService.h
index 37528947f52b3f399bbad44ba047e89070d536cd..edb52d6f0b15098aa3587e06c08a17d3cd05500d 100644
--- a/dfall_ws/src/dfall_pkg/include/nodes/PickerControllerService.h
+++ b/dfall_ws/src/dfall_pkg/include/nodes/PickerControllerService.h
@@ -379,5 +379,6 @@ bool getCurrentSetpointCallback(GetSetpointService::Request &request, GetSetpoin
void publish_current_xyz_yaw(float x, float y, float z, float yaw);
// FOR LOADING THE YAML PARAMETERS
+void timerCallback_initial_load_yaml(const ros::TimerEvent&);
void isReadyPickerControllerYamlCallback(const IntWithHeader & msg);
void fetchPickerControllerYamlParameters(ros::NodeHandle& nodeHandle);
\ No newline at end of file
diff --git a/dfall_ws/src/dfall_pkg/launch/agent.launch b/dfall_ws/src/dfall_pkg/launch/agent.launch
index 50b230067eceb4430e1108399d2eeaf0204011c7..34881f679f80318a181ed50bbb3974b5fea246db 100755
--- a/dfall_ws/src/dfall_pkg/launch/agent.launch
+++ b/dfall_ws/src/dfall_pkg/launch/agent.launch
@@ -56,24 +56,6 @@
>
</node>
- <!-- SAFE CONTROLLER -->
- <node
- pkg = "dfall_pkg"
- name = "SafeControllerService"
- output = "screen"
- type = "SafeControllerService"
- >
- </node>
-
- <!-- DEMO CONTROLLER -->
- <node
- pkg = "dfall_pkg"
- name = "DemoControllerService"
- output = "screen"
- type = "DemoControllerService"
- >
- </node>
-
<!-- STUDENT CONTROLLER -->
<node
pkg = "dfall_pkg"
diff --git a/dfall_ws/src/dfall_pkg/param/DefaultController.yaml b/dfall_ws/src/dfall_pkg/param/DefaultController.yaml
index bb232502ca80158afeb3f4e4f93ce53e4a26a744..52c150ef1723f86694e28896220c7ce76d756eae 100644
--- a/dfall_ws/src/dfall_pkg/param/DefaultController.yaml
+++ b/dfall_ws/src/dfall_pkg/param/DefaultController.yaml
@@ -42,7 +42,7 @@ takoff_goto_setpoint_max_time: 4.0
takoff_integrator_on_box_horizontal: 0.25
takoff_integrator_on_box_vertical: 0.15
# The time for: take off integrator-on
-takoff_integrator_on_time: 4.0
+takoff_integrator_on_time: 5.0
# Height change for the landing move-down
diff --git a/dfall_ws/src/dfall_pkg/src/nodes/DefaultControllerService.cpp b/dfall_ws/src/dfall_pkg/src/nodes/DefaultControllerService.cpp
index f05ce8008c6307398f7fd1948887ab1c6558c465..8d63aef825efdc413edbed16d435d4936a9c43f2 100644
--- a/dfall_ws/src/dfall_pkg/src/nodes/DefaultControllerService.cpp
+++ b/dfall_ws/src/dfall_pkg/src/nodes/DefaultControllerService.cpp
@@ -515,6 +515,13 @@ void computeResponse_for_takeoff_goto_setpoint(Controller::Response &response)
void computeResponse_for_takeoff_integrator_on(Controller::Response &response)
{
+ // Initialise static variables for only running the
+ // integrator once, and adjusting the time
+ // accordingly
+ static bool static_integrator_complete_once = false;
+ static float static_integrator_on_time = yaml_takoff_integrator_on_time;
+ static int static_integrator_flag = INTEGRATOR_FLAG_ON;
+
// Check if the state "just recently" changed
if (m_current_state_changed)
{
@@ -525,6 +532,17 @@ void computeResponse_for_takeoff_integrator_on(Controller::Response &response)
m_setpoint_for_controller[1] = m_setpoint[1];
m_setpoint_for_controller[2] = m_setpoint[2];
m_setpoint_for_controller[3] = m_setpoint[3];
+ // Adjust the integrator on/off and time
+ if (static_integrator_complete_once)
+ {
+ static_integrator_flag = INTEGRATOR_FLAG_OFF;
+ static_integrator_on_time = 0.5;
+ }
+ else
+ {
+ static_integrator_flag = INTEGRATOR_FLAG_ON;
+ static_integrator_on_time = yaml_takoff_integrator_on_time;
+ }
// Set the change flag back to false
m_current_state_changed = false;
// Inform the user
@@ -532,13 +550,15 @@ void computeResponse_for_takeoff_integrator_on(Controller::Response &response)
}
// Call the LQR control function
- calculateControlOutput_viaLQR_givenSetpoint(m_setpoint_for_controller, m_current_stateInertialEstimate, response , INTEGRATOR_FLAG_ON);
+ calculateControlOutput_viaLQR_givenSetpoint(m_setpoint_for_controller, m_current_stateInertialEstimate, response , static_integrator_flag);
// Change to next state after specified time
- if (m_time_in_seconds > yaml_takoff_integrator_on_time)
+ if (m_time_in_seconds > static_integrator_on_time)
{
// Inform the user
ROS_INFO("[DEFAULT CONTROLLER] Publish message that take-off is complete, and switch to state: normal");
+ // Set that the integrator cylce was completed
+ static_integrator_complete_once = true;
// Reset the time variable
m_time_in_seconds = 0.0;
// Update the state accordingly
@@ -1929,7 +1949,7 @@ int main(int argc, char* argv[])
// service call containing the filename of the *.yaml file,
// and then a message will be received on the above subscribers
// when the paramters are ready.
- // > NOTE IMPORTANTLY that by using a serice client
+ // > NOTE IMPORTANTLY that by using a service client
// we stall the availability of this node until the
// paramter service is ready
// > NOTE FURTHER that calling on the service directly from here
@@ -1937,7 +1957,7 @@ int main(int argc, char* argv[])
// instead use a timer to delay the loading
// Create a single-shot timer
- ros::Timer timer_initial_load_yaml = nodeHandle.createTimer(ros::Duration(1.0), timerCallback_initial_load_yaml, true);
+ ros::Timer timer_initial_load_yaml = nodeHandle.createTimer(ros::Duration(3.0), timerCallback_initial_load_yaml, true);
timer_initial_load_yaml.start();
diff --git a/dfall_ws/src/dfall_pkg/src/nodes/ParameterService.cpp b/dfall_ws/src/dfall_pkg/src/nodes/ParameterService.cpp
index 854b71acb5f26e98c8cbbbeae6ea92956dc8b577..269f0a55fdacbe0a1c9219a8ec2eeedbe6074e90 100644
--- a/dfall_ws/src/dfall_pkg/src/nodes/ParameterService.cpp
+++ b/dfall_ws/src/dfall_pkg/src/nodes/ParameterService.cpp
@@ -148,7 +148,7 @@ void requestLoadYamlFilenameCallback(const StringWithHeader& yaml_filename_to_lo
}
}
// Sleep to make the publisher known to ROS (in seconds)
- ros::Duration(1.0).sleep();
+ ros::Duration(2.0).sleep();
// Send the message
yamlParametersReadyForFetchPublisher.publish(yaml_ready_msg);
@@ -332,13 +332,13 @@ int main(int argc, char* argv[])
}
// Subscribe to the messages that request loading a yaml file
- ros::Subscriber requestLoadYamlFilenameSubscriber = nodeHandle.subscribe("requestLoadYamlFilename", 1, requestLoadYamlFilenameCallback);
+ ros::Subscriber requestLoadYamlFilenameSubscriber = nodeHandle.subscribe("requestLoadYamlFilename", 20, requestLoadYamlFilenameCallback);
// Publisher for publishing "internally" to the subscriber above
requestLoadYamlFilenamePublisher = nodeHandle.advertise <StringWithHeader>("requestLoadYamlFilename", 1);
// Advertise the service for requests to load a yaml file
- ros::ServiceServer getCurrentFlyingStateService = nodeHandle.advertiseService("requestLoadYamlFilename", requestLoadYamlFilenameCallbackServiceCallback);
+ ros::ServiceServer requestLoadYamlFilenameService = nodeHandle.advertiseService("requestLoadYamlFilename", requestLoadYamlFilenameCallbackServiceCallback);
// Inform the user the this node is ready
ROS_INFO("[PARAMETER SERVICE] Service ready :-)");
diff --git a/dfall_ws/src/dfall_pkg/src/nodes/PickerControllerService.cpp b/dfall_ws/src/dfall_pkg/src/nodes/PickerControllerService.cpp
index 6d28bc65675b30c43d80ab94240d15cbb646d92a..4a687a4be0743590797ec21125de3dc26102f158 100644
--- a/dfall_ws/src/dfall_pkg/src/nodes/PickerControllerService.cpp
+++ b/dfall_ws/src/dfall_pkg/src/nodes/PickerControllerService.cpp
@@ -1087,6 +1087,36 @@ void publish_current_xyz_yaw(float x, float y, float z, float yaw)
// ----------------------------------------------------------------------------------
+// TIMER CALLBACK FOR SENDING THE LOAD YAML REQUEST
+void timerCallback_initial_load_yaml(const ros::TimerEvent&)
+{
+ // Create a node handle to the selected parameter service
+ ros::NodeHandle nodeHandle_to_own_agent_parameter_service(m_namespace_to_own_agent_parameter_service);
+ // Create the service client as a local variable
+ ros::ServiceClient requestLoadYamlFilenameServiceClient = nodeHandle_to_own_agent_parameter_service.serviceClient<LoadYamlFromFilename>("requestLoadYamlFilename", false);
+ // Create the service call as a local variable
+ LoadYamlFromFilename loadYamlFromFilenameCall;
+ // Specify the Yaml filename as a string
+ loadYamlFromFilenameCall.request.stringWithHeader.data = "PickerController";
+ // Set for whom this applies to
+ loadYamlFromFilenameCall.request.stringWithHeader.shouldCheckForAgentID = false;
+ // Wait until the serivce exists
+ requestLoadYamlFilenameServiceClient.waitForExistence(ros::Duration(-1));
+ // Make the service call
+ if(requestLoadYamlFilenameServiceClient.call(loadYamlFromFilenameCall))
+ {
+ // Nothing to do in this case.
+ // The "isReadyDefaultControllerYamlCallback" function
+ // will be called once the YAML file is loaded
+ }
+ else
+ {
+ // Inform the user
+ ROS_ERROR("[DEFAULT CONTROLLER] The request load yaml file service call failed.");
+ }
+}
+
+
// LOADING OF YAML PARAMETERS
void isReadyPickerControllerYamlCallback(const IntWithHeader & msg)
{
@@ -1338,32 +1368,16 @@ int main(int argc, char* argv[]) {
// service call containing the filename of the *.yaml file,
// and then a message will be received on the above subscribers
// when the paramters are ready.
- // > NOTE IMPORTANTLY that by using a serice client
+ // > NOTE IMPORTANTLY that by using a service client
// we stall the availability of this node until the
// paramter service is ready
+ // > NOTE FURTHER that calling on the service directly from here
+ // often means the yaml file is not actually loaded. So we
+ // instead use a timer to delay the loading
- // Create the service client as a local variable
- ros::ServiceClient requestLoadYamlFilenameServiceClient = nodeHandle_to_own_agent_parameter_service.serviceClient<LoadYamlFromFilename>("requestLoadYamlFilename", false);
- // Create the service call as a local variable
- LoadYamlFromFilename loadYamlFromFilenameCall;
- // Specify the Yaml filename as a string
- loadYamlFromFilenameCall.request.stringWithHeader.data = "PickerController";
- // Set for whom this applies to
- loadYamlFromFilenameCall.request.stringWithHeader.shouldCheckForAgentID = false;
- // Wait until the serivce exists
- requestLoadYamlFilenameServiceClient.waitForExistence(ros::Duration(-1));
- // Make the service call
- if(requestLoadYamlFilenameServiceClient.call(loadYamlFromFilenameCall))
- {
- // Nothing to do in this case.
- // The "isReadyPickerControllerYamlCallback" function
- // will be called once the YAML file is loaded
- }
- else
- {
- // Inform the user
- ROS_ERROR("[PICKER CONTROLLER] The request load yaml file service call failed.");
- }
+ // Create a single-shot timer
+ ros::Timer timer_initial_load_yaml = nodeHandle.createTimer(ros::Duration(4.0), timerCallback_initial_load_yaml, true);
+ timer_initial_load_yaml.start();