From d9fd6069c1f432de5d17a6fd918f3698c20bd6bc Mon Sep 17 00:00:00 2001
From: beuchatp <beuchatp@control.ee.ethz.ch>
Date: Thu, 30 Nov 2017 13:30:17 +0100
Subject: [PATCH] Loading of parameters from PPSClient, SafeController, and
Custom controller now working, still need to debug messages from GUI buttons
---
.../GUI_Qt/studentGUI/src/MainWindow.cpp | 32 +++++++------
.../src/CustomControllerService.cpp | 8 ++--
pps_ws/src/d_fall_pps/src/PPSClient.cpp | 46 +++++++++----------
.../src/d_fall_pps/src/ParameterService.cpp | 18 ++++++--
.../d_fall_pps/src/SafeControllerService.cpp | 10 ++--
5 files changed, 64 insertions(+), 50 deletions(-)
diff --git a/pps_ws/src/d_fall_pps/GUI_Qt/studentGUI/src/MainWindow.cpp b/pps_ws/src/d_fall_pps/GUI_Qt/studentGUI/src/MainWindow.cpp
index 6589f8bb..9fab80fb 100644
--- a/pps_ws/src/d_fall_pps/GUI_Qt/studentGUI/src/MainWindow.cpp
+++ b/pps_ws/src/d_fall_pps/GUI_Qt/studentGUI/src/MainWindow.cpp
@@ -56,7 +56,7 @@ MainWindow::MainWindow(int argc, char **argv, QWidget *parent) :
setCrazyRadioStatus(DISCONNECTED);
m_ros_namespace = ros::this_node::getNamespace();
- ROS_INFO("namespace: %s", m_ros_namespace.c_str());
+ ROS_INFO("Student GUI node namespace: %s", m_ros_namespace.c_str());
qRegisterMetaType<ptrToMessage>("ptrToMessage");
QObject::connect(m_rosNodeThread, SIGNAL(newViconData(const ptrToMessage&)), this, SLOT(updateNewViconData(const ptrToMessage&)));
@@ -84,18 +84,22 @@ MainWindow::MainWindow(int argc, char **argv, QWidget *parent) :
ros::NodeHandle my_nodeHandle("~");
controllerSetpointPublisher = my_nodeHandle.advertise<Setpoint>("ControllerSetpoint", 1);
-// > For publishing a message that requests the
- // YAML parameters to be re-loaded from file
- // > The message contents specify which controller
- // the parameters should be re-loaded for
- requestLoadControllerYamlPublisher = my_nodeHandle.advertise<std_msgs::Int32>("requestLoadControllerYaml", 1);
+
// communication with PPS Client, just to make it possible to communicate through terminal also we use PPSClient's name
- ros::NodeHandle nh_PPSClient(m_ros_namespace + "/PPSClient");
+ //ros::NodeHandle nh_PPSClient(m_ros_namespace + "/PPSClient");
+ ros::NodeHandle nh_PPSClient("PPSClient");
crazyRadioCommandPublisher = nh_PPSClient.advertise<std_msgs::Int32>("crazyRadioCommand", 1);
PPSClientCommandPublisher = nh_PPSClient.advertise<std_msgs::Int32>("Command", 1);
+
+ // > For publishing a message that requests the
+ // YAML parameters to be re-loaded from file
+ // > The message contents specify which controller
+ // the parameters should be re-loaded for
+ requestLoadControllerYamlPublisher = nh_PPSClient.advertise<std_msgs::Int32>("requestLoadControllerYaml", 1);
+
// Subscriber for locking the load the controller YAML
@@ -115,22 +119,24 @@ MainWindow::MainWindow(int argc, char **argv, QWidget *parent) :
- std::vector<float> default_setpoint(4);
- ros::NodeHandle nh_safeControllerService(m_ros_namespace + "/SafeControllerService");
- ROS_INFO_STREAM(m_ros_namespace << "/SafeControllerService");
+ // Load default setpoint from the "SafeController" namespace of the "ParameterService"
+ std::vector<float> default_setpoint(4);
+ ros::NodeHandle nodeHandle_to_own_agent_parameter_service("ParameterService");
+ ros::NodeHandle nodeHandle_for_safeController(nodeHandle_to_own_agent_parameter_service, "SafeController");
- if(!nh_safeControllerService.getParam("defaultSetpoint", default_setpoint))
+ if(!nodeHandle_for_safeController.getParam("defaultSetpoint", default_setpoint))
{
- ROS_ERROR_STREAM("couldn't find parameter 'defaultSetpoint'");
+ ROS_ERROR_STREAM("The StudentGUI could not find parameter 'defaultSetpoint', as called from main(...)");
}
-
+ // Copy the default setpoint into respective text fields of the GUI
ui->current_setpoint_x->setText(QString::number(default_setpoint[0]));
ui->current_setpoint_y->setText(QString::number(default_setpoint[1]));
ui->current_setpoint_z->setText(QString::number(default_setpoint[2]));
ui->current_setpoint_yaw->setText(QString::number(default_setpoint[3]));
+
disableGUI();
highlightSafeControllerTab();
ui->label_battery->setStyleSheet("QLabel { color : red; }");
diff --git a/pps_ws/src/d_fall_pps/src/CustomControllerService.cpp b/pps_ws/src/d_fall_pps/src/CustomControllerService.cpp
index 11c1d6a7..eae858d8 100644
--- a/pps_ws/src/d_fall_pps/src/CustomControllerService.cpp
+++ b/pps_ws/src/d_fall_pps/src/CustomControllerService.cpp
@@ -857,7 +857,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"
- ros::NodeHandle nodeHandle_for_customController = ros::NodeHandle(nodeHandle, "/CustomController");
+ ros::NodeHandle nodeHandle_for_customController(nodeHandle, "CustomController");
// > The mass of the crazyflie
cf_mass = getParameterFloat(nodeHandle_for_customController , "mass");
@@ -1142,15 +1142,15 @@ int main(int argc, char* argv[]) {
// Set the class variable "namespace_to_own_agent_parameter_service" to be a the
// namespace string for the parameter service that is running on the machine of this
// agent
- namespace_to_own_agent_parameter_service = (m_namespace + "/ParameterService");
+ namespace_to_own_agent_parameter_service = "ParameterService";
// Create a node handle to the parameter service running on this agent's machine
ros::NodeHandle nodeHandle_to_own_agent_parameter_service = ros::NodeHandle(namespace_to_own_agent_parameter_service);
// 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
- std::string m_ros_namespace = ros::getNamespace();
- namespace_to_coordinator_parameter_service = (m_ros_namespace + "/ParameterService");
+ //std::string m_ros_namespace = ros::getNamespace();
+ namespace_to_coordinator_parameter_service = "ParameterService";
// Create a node handle to the parameter service running on the coordinator machine
ros::NodeHandle nodeHandle_to_coordinator_parameter_service = ros::NodeHandle(namespace_to_own_agent_parameter_service);
diff --git a/pps_ws/src/d_fall_pps/src/PPSClient.cpp b/pps_ws/src/d_fall_pps/src/PPSClient.cpp
index 968aa9dc..cc0f05fd 100755
--- a/pps_ws/src/d_fall_pps/src/PPSClient.cpp
+++ b/pps_ws/src/d_fall_pps/src/PPSClient.cpp
@@ -809,7 +809,7 @@ void yamlReadyForFetchCallback(const std_msgs::Int32& msg)
// Let the user know which paramters are being fetch
ROS_INFO("> Now fetching the parameter values from the this machine");
// Create a node handle to the parameter service running on this agent's machine
- ros::NodeHandle nodeHandle_to_own_agent_parameter_service = ros::NodeHandle(namespace_to_own_agent_parameter_service);
+ ros::NodeHandle nodeHandle_to_own_agent_parameter_service(namespace_to_own_agent_parameter_service);
// Call the function that fetches the parameters
fetchYamlParametersForSafeController(nodeHandle_to_own_agent_parameter_service);
break;
@@ -832,8 +832,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"
- ros::NodeHandle nodeHandle_for_safeController = ros::NodeHandle(nodeHandle, "/SafeController");
-
+ ros::NodeHandle nodeHandle_for_safeController(nodeHandle, "SafeController");
if(!nodeHandle_for_safeController.getParam("takeOffDistance", take_off_distance))
{
@@ -999,23 +998,6 @@ int main(int argc, char* argv[])
ros::NodeHandle nodeHandle("~");
ros_namespace = ros::this_node::getNamespace();
- // load default setpoint
- std::vector<float> default_setpoint(4);
- ros::NodeHandle nh_safeControllerService(ros_namespace + "/SafeControllerService");
-
- ROS_INFO_STREAM(ros_namespace << "/SafeControllerService");
-
- if(!nh_safeControllerService.getParam("defaultSetpoint", default_setpoint))
- {
- ROS_ERROR_STREAM("couldn't find parameter 'defaultSetpoint'");
- }
-
- controller_setpoint.x = default_setpoint[0];
- controller_setpoint.y = default_setpoint[1];
- controller_setpoint.z = default_setpoint[2];
- controller_setpoint.yaw = default_setpoint[3];
-
-
// *********************************************************************************
// EVERYTHING THAT RELATES TO FETCHING PARAMETERS FROM A YAML FILE
@@ -1028,15 +1010,15 @@ int main(int argc, char* argv[])
// Set the class variable "namespace_to_own_agent_parameter_service" to be a the
// namespace string for the parameter service that is running on the machine of this
// agent
- namespace_to_own_agent_parameter_service = (m_namespace + "/ParameterService");
+ namespace_to_own_agent_parameter_service = "ParameterService";
// Create a node handle to the parameter service running on this agent's machine
- ros::NodeHandle nodeHandle_to_own_agent_parameter_service = ros::NodeHandle(namespace_to_own_agent_parameter_service);
+ ros::NodeHandle nodeHandle_to_own_agent_parameter_service(namespace_to_own_agent_parameter_service);
// 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
- std::string m_ros_namespace = ros::getNamespace();
- namespace_to_coordinator_parameter_service = (m_ros_namespace + "/ParameterService");
+ //std::string m_ros_namespace = ros::getNamespace();
+ namespace_to_coordinator_parameter_service = "ParameterService";
// Create a node handle to the parameter service running on the coordinator machine
ros::NodeHandle nodeHandle_to_coordinator_parameter_service = ros::NodeHandle(namespace_to_own_agent_parameter_service);
@@ -1059,6 +1041,22 @@ int main(int argc, char* argv[])
fetchYamlParametersForSafeController(nodeHandle_to_own_agent_parameter_service);
// *********************************************************************************
+
+
+ // Load default setpoint from the "SafeController" namespace of the "ParameterService"
+ std::vector<float> default_setpoint(4);
+ ros::NodeHandle nodeHandle_for_safeController(nodeHandle_to_own_agent_parameter_service, "SafeController");
+
+ if(!nodeHandle_for_safeController.getParam("defaultSetpoint", default_setpoint))
+ {
+ ROS_ERROR_STREAM("The PPSClient could not find parameter 'defaultSetpoint', as called from main(...)");
+ }
+
+ // Copy the default setpoint into the class variable
+ controller_setpoint.x = default_setpoint[0];
+ controller_setpoint.y = default_setpoint[1];
+ controller_setpoint.z = default_setpoint[2];
+ controller_setpoint.yaw = default_setpoint[3];
//ros::service::waitForService("/CentralManagerService/CentralManager");
diff --git a/pps_ws/src/d_fall_pps/src/ParameterService.cpp b/pps_ws/src/d_fall_pps/src/ParameterService.cpp
index be0b3f66..7be7f237 100755
--- a/pps_ws/src/d_fall_pps/src/ParameterService.cpp
+++ b/pps_ws/src/d_fall_pps/src/ParameterService.cpp
@@ -169,6 +169,8 @@ void requestLoadControllerYamlCallback(const std_msgs::Int32& msg)
// parameters should be loaded
int controller_to_load_yaml = msg.data;
+ ROS_INFO_STREAM("The Parameter Service node received the message to load YAML parameters from file into cache");
+
// Instantiate a local varaible to confirm that something was actually loaded from
// a YAML file
@@ -342,12 +344,20 @@ int main(int argc, char* argv[])
case TYPE_AGENT:
{
// Get the node handles required
- ros::NodeHandle agent_nodeHandle = ros::NodeHandle();
- ros::NodeHandle agent_namespace_nodeHandle(ros::this_node::getNamespace());
+ //ros::NodeHandle agent_nodeHandle = ros::NodeHandle();
+ //ros::NodeHandle agent_namespace_nodeHandle(ros::this_node::getNamespace());
// > Subscribe to requests from: the master GUI
- ros::Subscriber requestLoadControllerYamlSubscriber_agent_to_master = agent_nodeHandle.subscribe("/my_GUI/requestLoadControllerYaml", 1, requestLoadControllerYamlCallback);
+ //ros::NodeHandle nh_studentGUI_for_this_agent("student_GUI");
+ //ros::Subscriber requestLoadControllerYamlSubscriber_agent_to_master = nh_studentGUI_for_this_agent.subscribe("requestLoadControllerYaml", 1, requestLoadControllerYamlCallback);
// > Subscribe to requests from: the agent's own "PPSClient" node
- ros::Subscriber requestLoadControllerYamlSubscriber_agent_to_self = agent_namespace_nodeHandle.subscribe("PPSClient/requestLoadControllerYaml", 1, requestLoadControllerYamlCallback);
+ //std::string temp_this_node_namespace = ros::this_node::getNamespace();
+ //ros::NodeHandle nh_PPSClient_for_this_agent(temp_this_node_namespace + "/PPSClient");
+ ros::NodeHandle nh_PPSClient_for_this_agent(nodeHandle,"PPSClient");
+ ros::Subscriber requestLoadControllerYamlSubscriber_agent_to_self = nh_PPSClient_for_this_agent.subscribe("requestLoadControllerYaml", 1, requestLoadControllerYamlCallback);
+
+
+ ROS_INFO_STREAM("This Parameter Service has subscribed to 'requestLoadControllerYaml' messages from both the 'my_GUI' and the 'PPSClient'");
+
break;
}
diff --git a/pps_ws/src/d_fall_pps/src/SafeControllerService.cpp b/pps_ws/src/d_fall_pps/src/SafeControllerService.cpp
index d7f39c51..89a6a96b 100755
--- a/pps_ws/src/d_fall_pps/src/SafeControllerService.cpp
+++ b/pps_ws/src/d_fall_pps/src/SafeControllerService.cpp
@@ -425,7 +425,7 @@ 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"
- ros::NodeHandle nodeHandle_for_safeController = ros::NodeHandle(nodeHandle, "/SafeController");
+ ros::NodeHandle nodeHandle_for_safeController(nodeHandle, "SafeController");
// > The mass of the crazyflie
cf_mass = getParameterFloat(nodeHandle_for_safeController, "mass");
@@ -586,15 +586,15 @@ int main(int argc, char* argv[]) {
// Set the class variable "namespace_to_own_agent_parameter_service" to be a the
// namespace string for the parameter service that is running on the machine of this
// agent
- namespace_to_own_agent_parameter_service = (m_namespace + "/ParameterService");
+ namespace_to_own_agent_parameter_service = "ParameterService";
// Create a node handle to the parameter service running on this agent's machine
- ros::NodeHandle nodeHandle_to_own_agent_parameter_service = ros::NodeHandle(namespace_to_own_agent_parameter_service);
+ ros::NodeHandle nodeHandle_to_own_agent_parameter_service(namespace_to_own_agent_parameter_service);
// 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
- std::string m_ros_namespace = ros::getNamespace();
- namespace_to_coordinator_parameter_service = (m_ros_namespace + "/ParameterService");
+ //std::string m_ros_namespace = ros::master::getNamespace();
+ namespace_to_coordinator_parameter_service = "ParameterService";
// Create a node handle to the parameter service running on the coordinator machine
ros::NodeHandle nodeHandle_to_coordinator_parameter_service = ros::NodeHandle(namespace_to_own_agent_parameter_service);
--
GitLab