Commit 0a611958 authored by beuchatp's avatar beuchatp
Browse files

Debugged the addition of a Parameter Service

parent ce54494b
......@@ -1056,12 +1056,12 @@ void MainGUIWindow::on_all_enable_custom_controller_button_clicked()
void MainGUIWindow::on_all_load_safe_controller_yaml_own_agent_button_clicked()
{
// Disable the button
ui->all_load_safe_controller_yaml_button->setEnabled(false);
ui->all_send_safe_controller_yaml_button->setEnabled(false);
ui->all_load_safe_controller_yaml_own_agent_button->setEnabled(false);
ui->all_load_safe_controller_yaml_coordinator_button->setEnabled(false);
// Send the message that the YAML paremters should be loaded
std_msgs::Int32 msg;
msg.data = LOAD_YAML_SAFE_CONTROLLER_OWN_AGENT;
msg.data = LOAD_YAML_SAFE_CONTROLLER_AGENT;
requestLoadControllerYamlPublisher.publish(msg);
// Start a timer which will enable the button in its callback
......@@ -1077,12 +1077,12 @@ void MainGUIWindow::on_all_load_safe_controller_yaml_own_agent_button_clicked()
void MainGUIWindow::on_all_load_custom_controller_yaml_own_agent_button_clicked()
{
// Disable the button
ui->all_load_custom_controller_yaml_button->setEnabled(false);
ui->all_send_custom_controller_yaml_button->setEnabled(false);
ui->all_load_custom_controller_yaml_own_agent_button->setEnabled(false);
ui->all_load_custom_controller_yaml_coordinator_button->setEnabled(false);
// Send the message that the YAML paremters should be loaded
std_msgs::Int32 msg;
msg.data = LOAD_YAML_CUSTOM_CONTROLLER_OWN_AGENT;
msg.data = LOAD_YAML_CUSTOM_CONTROLLER_AGENT;
requestLoadControllerYamlPublisher.publish(msg);
// Start a timer which will enable the button in its callback
......@@ -1099,8 +1099,8 @@ void MainGUIWindow::on_all_load_custom_controller_yaml_own_agent_button_clicked(
void MainGUIWindow::on_all_load_safe_controller_yaml_coordinator_button_clicked()
{
// Disable the button
ui->all_load_safe_controller_yaml_button->setEnabled(false);
ui->all_send_safe_controller_yaml_button->setEnabled(false);
ui->all_load_safe_controller_yaml_own_agent_button->setEnabled(false);
ui->all_load_safe_controller_yaml_coordinator_button->setEnabled(false);
// Send the message that the YAML paremters should be loaded
// by the coordinator (and then the agent informed)
......@@ -1121,8 +1121,8 @@ void MainGUIWindow::on_all_load_safe_controller_yaml_coordinator_button_clicked(
void MainGUIWindow::on_all_load_custom_controller_yaml_coordinator_button_clicked()
{
// Disable the button
ui->all_load_custom_controller_yaml_button->setEnabled(false);
ui->all_send_custom_controller_yaml_button->setEnabled(false);
ui->all_load_custom_controller_yaml_own_agent_button->setEnabled(false);
ui->all_load_custom_controller_yaml_coordinator_button->setEnabled(false);
// Send the message that the YAML paremters should be loaded
// by the coordinator (and then the agent informed)
......@@ -1137,23 +1137,22 @@ void MainGUIWindow::on_all_load_custom_controller_yaml_coordinator_button_clicke
// > Thus we use this timer to prevent the user from clicking the
// button in the GUI repeatedly.
ros::NodeHandle nodeHandle("~");
m_timer_yaml_file_for_custom_controller = nodeHandle.createTimer(ros::Duration(1), &MainGUIWindow::customSendYamlAsMessageTimerCallback, this, true);
m_timer_yaml_file_for_custom_controller = nodeHandle.createTimer(ros::Duration(1.5), &MainGUIWindow::customYamlFileTimerCallback, this, true);
}
// > CALLBACK TO RE-ENABLE THE SAFE CONTROLLER YAML BUTTONS
void MainGUIWindow::safeYamlFileTimerCallback(const ros::TimerEvent&)
{
// Enble the "load" and the "send" safe controller YAML button again
ui->all_load_safe_controller_yaml_button->setEnabled(true);
ui->all_send_safe_controller_yaml_button->setEnabled(true);
ui->all_load_safe_controller_yaml_own_agent_button->setEnabled(true);
ui->all_load_safe_controller_yaml_coordinator_button->setEnabled(true);
}
// > CALLBACK TO RE-ENABLE THE CUSTOM CONTROLLER YAML BUTTONS
void MainGUIWindow::customYamlFileTimerCallback(const ros::TimerEvent&)
{
// Enble the "load" and the "send" custom controller YAML button again
ui->all_load_custom_controller_yaml_button->setEnabled(true);
ui->all_send_custom_controller_yaml_button->setEnabled(true);
ui->all_load_custom_controller_yaml_own_agent_button->setEnabled(true);
ui->all_load_custom_controller_yaml_coordinator_button->setEnabled(true);
}
/*
......
......@@ -573,7 +573,8 @@ void MainWindow::requestLoadControllerYaml_from_my_GUI_Callback(const std_msgs::
// Switch between loading for the different controllers
switch(controller_to_load_yaml)
{
case LOAD_YAML_SAFE_CONTROLLER_AGENT || LOAD_YAML_SAFE_CONTROLLER_COORDINATOR:
case LOAD_YAML_SAFE_CONTROLLER_AGENT:
case LOAD_YAML_SAFE_CONTROLLER_COORDINATOR:
// Set the "load safe yaml" button to be disabled
ui->load_safe_yaml_button->setEnabled(false);
......@@ -587,7 +588,8 @@ void MainWindow::requestLoadControllerYaml_from_my_GUI_Callback(const std_msgs::
break;
case LOAD_YAML_CUSTOM_CONTROLLER_AGENT || LOAD_YAML_CUSTOM_CONTROLLER_COORDINATOR:
case LOAD_YAML_CUSTOM_CONTROLLER_AGENT:
case LOAD_YAML_CUSTOM_CONTROLLER_COORDINATOR:
// Set the "load custom yaml" button to be disabled
ui->load_custom_yaml_button->setEnabled(false);
......
......@@ -8,8 +8,14 @@
<!-- When we have a GUI, this has to be adapted and included -->
<node pkg="d_fall_pps" name="my_GUI" output="screen" type="my_GUI">
<!-- <rosparam command="load" file="$(find d_fall_pps)/launch/ControlParams.yaml" /> -->
<rosparam command="load" file="$(find d_fall_pps)/param/CustomController.yaml" />
</node>
<!-- PARAMETER SERVICE -->
<node pkg="d_fall_pps" name="ParameterService" output="screen" type="ParameterService">
<param name="type" type="str" value="student" />
<param name="agentID" value="$(optenv ROS_NAMESPACE)" />
<rosparam command="load" file="$(find d_fall_pps)/param/SafeController.yaml" ns="SafeController" />
<rosparam command="load" file="$(find d_fall_pps)/param/CustomController.yaml" ns="CustomController" />
</node>
</launch>
......@@ -847,7 +847,7 @@ void fetchYamlParameters(ros::NodeHandle& nodeHandle)
// Here we load the parameters that are specified in the CustomController.yaml file
// Add the "CustomController" namespace to the "nodeHandle"
nodeHandle_for_customController = ros::NodeHandle(nodeHandle + "/CustomController");
ros::NodeHandle nodeHandle_for_customController = ros::NodeHandle(nodeHandle, "/CustomController");
// > The mass of the crazyflie
cf_mass = getParameterFloat(nodeHandle_for_customController , "mass");
......@@ -1136,7 +1136,7 @@ int main(int argc, char* argv[]) {
// Set the class variable "nodeHandle_to_coordinator_parameter_service" to be a node handle
// for the parameter service that is running on the coordinate machine
ros::NodeHandle coordinator_nodeHandle = ros::NodeHandle();
nodeHandle_to_coordinator_parameter_service = ros::NodeHandle(coordinator_nodeHandle + "/ParameterService");
nodeHandle_to_coordinator_parameter_service = ros::NodeHandle(coordinator_nodeHandle, "/ParameterService");
// Instantiate the local variable "controllerYamlReadyForFetchSubscriber" to be a
// "ros::Subscriber" type variable that subscribes to the "controllerYamlReadyForFetch" topic
......
......@@ -192,6 +192,13 @@ ros::Publisher commandPublisher;
ros::Publisher crazyRadioCommandPublisher;
// Variable for the node handle to the paramter services
// > For the paramter service of this agent
ros::NodeHandle nodeHandle_to_own_agent_parameter_service;
// > For the parameter service of the coordinator
ros::NodeHandle nodeHandle_to_coordinator_parameter_service;
// variables for the states:
int flying_state;
bool changed_state_flag;
......@@ -815,7 +822,7 @@ void fetchYamlParametersForSafeController(ros::NodeHandle& nodeHandle)
// Here we load the parameters that are specified in the SafeController.yaml file
// Add the "CustomController" namespace to the "nodeHandle"
nodeHandle_for_safeController = ros::NodeHandle(nodeHandle + "/SafeController");
ros::NodeHandle nodeHandle_for_safeController = ros::NodeHandle(nodeHandle, "/SafeController");
if(!nodeHandle_for_safeController.getParam("takeOffDistance", take_off_distance))
......@@ -1015,7 +1022,7 @@ int main(int argc, char* argv[])
// Set the class variable "nodeHandle_to_coordinator_parameter_service" to be a node handle
// for the parameter service that is running on the coordinate machine
ros::NodeHandle coordinator_nodeHandle = ros::NodeHandle();
nodeHandle_to_coordinator_parameter_service = ros::NodeHandle(coordinator_nodeHandle + "/ParameterService");
nodeHandle_to_coordinator_parameter_service = ros::NodeHandle(coordinator_nodeHandle, "/ParameterService");
// Instantiate the local variable "controllerYamlReadyForFetchSubscriber" to be a
// "ros::Subscriber" type variable that subscribes to the "controllerYamlReadyForFetch" topic
......@@ -1032,8 +1039,7 @@ int main(int argc, char* argv[])
ros::Subscriber controllerYamlReadyForFetchSubscriber_to_coordinator = nodeHandle_to_coordinator_parameter_service.subscribe("controllerYamlReadyForFetch", 1, yamlReadyForFetchCallback);
// Call the class function that loads the parameters for this class.
fetchYamlParameters(nodeHandle_to_own_agent_parameter_service);
loadSafeControllerParameters();
fetchYamlParametersForSafeController(nodeHandle_to_own_agent_parameter_service);
// *********************************************************************************
......
......@@ -415,31 +415,31 @@ void fetchYamlParameters(ros::NodeHandle& nodeHandle)
// Here we load the parameters that are specified in the SafeController.yaml file
// Add the "CustomController" namespace to the "nodeHandle"
nodeHandle_for_safeController = ros::NodeHandle(nodeHandle + "/SafeController");
ros::NodeHandle nodeHandle_for_safeController = ros::NodeHandle(nodeHandle, "/SafeController");
// > The mass of the crazyflie
cf_mass = getParameterFloat(nodeHandle_for_safeController, "mass");
// > The co-efficients of the quadratic conversation from 16-bit motor command to
// thrust force in Newtons
loadParameterFloatVector(nodeHandle_for_safeController, "motorPoly", motorPoly, 3);
getParameterFloatVector(nodeHandle_for_safeController, "motorPoly", motorPoly, 3);
// > The row of the LQR matrix that commands body frame roll rate
loadParameterFloatVector(nodeHandle_for_safeController, "gainMatrixRoll", gainMatrixRoll, 9);
getParameterFloatVector(nodeHandle_for_safeController, "gainMatrixRoll", gainMatrixRoll, 9);
// > The row of the LQR matrix that commands body frame pitch rate
loadParameterFloatVector(nodeHandle_for_safeController, "gainMatrixPitch", gainMatrixPitch, 9);
getParameterFloatVector(nodeHandle_for_safeController, "gainMatrixPitch", gainMatrixPitch, 9);
// > The row of the LQR matrix that commands body frame yaw rate
loadParameterFloatVector(nodeHandle_for_safeController, "gainMatrixYaw", gainMatrixYaw, 9);
getParameterFloatVector(nodeHandle_for_safeController, "gainMatrixYaw", gainMatrixYaw, 9);
// > The row of the LQR matrix that commands thrust adjustment
loadParameterFloatVector(nodeHandle_for_safeController, "gainMatrixThrust", gainMatrixThrust, 9);
getParameterFloatVector(nodeHandle_for_safeController, "gainMatrixThrust", gainMatrixThrust, 9);
// > The gains for the point-mass Kalman filter
loadParameterFloatVector(nodeHandle_for_safeController, "filterGain", filterGain, 6);
loadParameterFloatVector(nodeHandle_for_safeController, "estimatorMatrix", estimatorMatrix, 2);
getParameterFloatVector(nodeHandle_for_safeController, "filterGain", filterGain, 6);
getParameterFloatVector(nodeHandle_for_safeController, "estimatorMatrix", estimatorMatrix, 2);
// > The defailt setpoint of the controller
loadParameterFloatVector(nodeHandle_for_safeController, "defaultSetpoint", defaultSetpoint, 4);
getParameterFloatVector(nodeHandle_for_safeController, "defaultSetpoint", defaultSetpoint, 4);
// Call the function that computes details an values that are needed from these
// parameters loaded above
......@@ -580,7 +580,7 @@ int main(int argc, char* argv[]) {
// Set the class variable "nodeHandle_to_coordinator_parameter_service" to be a node handle
// for the parameter service that is running on the coordinate machine
ros::NodeHandle coordinator_nodeHandle = ros::NodeHandle();
nodeHandle_to_coordinator_parameter_service = ros::NodeHandle(coordinator_nodeHandle + "/ParameterService");
nodeHandle_to_coordinator_parameter_service = ros::NodeHandle(coordinator_nodeHandle, "/ParameterService");
// Instantiate the local variable "controllerYamlReadyForFetchSubscriber" to be a
// "ros::Subscriber" type variable that subscribes to the "controllerYamlReadyForFetch" topic
......
Supports Markdown
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