To receive notifications about scheduled maintenance, please subscribe to the mailing-list gitlab-operations@sympa.ethz.ch. You can subscribe to the mailing-list at https://sympa.ethz.ch

Commit 3aed53eb authored by beuchatp's avatar beuchatp
Browse files

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
parent 78e55fe4
......@@ -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
......
......@@ -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
......@@ -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"
......
......@@ -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
......
......@@ -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();
......
......@@ -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 :-)");
......
......@@ -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();
......
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment