Minor fixes arising from the first practical class of Spring semester 2019....

Minor fixes arising from the first practical class of Spring semester 2019. Mainly relating to loading of paramters on first run, and also the integrator of the default controller

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

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

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"

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

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();
 	
 

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

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();