From 554cb7f7024833d7aae0d3a8e8dafc9cfd2707c7 Mon Sep 17 00:00:00 2001 From: Paul Beuchat <beuchatp@control.ee.ethz.ch> Date: Fri, 29 Nov 2019 09:30:08 +0100 Subject: [PATCH] Adjust the flying agent client messages and services to start with a Capital letter --- .../flyingAgentGUI/src/connectstartstopbar.cpp | 12 ++++++------ .../GUI_Qt/flyingAgentGUI/src/controllertabs.cpp | 10 +++++----- .../GUI_Qt/flyingAgentGUI/src/coordinatorrow.cpp | 12 ++++++------ .../GUI_Qt/flyingAgentGUI/src/csonecontrollertab.cpp | 6 +++--- .../flyingAgentGUI/src/defaultcontrollertab.cpp | 4 ++-- .../flyingAgentGUI/src/pickercontrollertab.cpp | 4 ++-- .../flyingAgentGUI/src/remotecontrollertab.cpp | 4 ++-- .../flyingAgentGUI/src/studentcontrollertab.cpp | 4 ++-- .../flyingAgentGUI/src/templatecontrollertab.cpp | 4 ++-- .../GUI_Qt/flyingAgentGUI/src/topbanner.cpp | 4 ++-- .../flyingAgentGUI/src/tuningcontrollertab.cpp | 4 ++-- .../GUI_Qt/systemConfigGUI/src/mainguiwindow.cpp | 2 +- dfall_ws/src/dfall_pkg/crazyradio/CrazyRadio.py | 4 ++-- dfall_ws/src/dfall_pkg/crazyradio/TestCF.py | 2 +- .../src/dfall_pkg/src/classes/QuadrotorSimulator.cpp | 2 +- dfall_ws/src/dfall_pkg/src/nodes/BatteryMonitor.cpp | 2 +- .../src/dfall_pkg/src/nodes/CrazyRadioEmulator.cpp | 4 ++-- .../src/dfall_pkg/src/nodes/FlyingAgentClient.cpp | 12 ++++++------ 18 files changed, 48 insertions(+), 48 deletions(-) diff --git a/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/src/connectstartstopbar.cpp b/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/src/connectstartstopbar.cpp index c82fc96b..fda9d841 100644 --- a/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/src/connectstartstopbar.cpp +++ b/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/src/connectstartstopbar.cpp @@ -106,7 +106,7 @@ ConnectStartStopBar::ConnectStartStopBar(QWidget *parent) : // SUBSCRIBERS AND PUBLISHERS: // > For Crazyradio commands based on button clicks - crazyRadioCommandPublisher = base_nodeHandle.advertise<dfall_pkg::IntWithHeader>("FlyingAgentClient/crazyRadioCommand", 1); + crazyRadioCommandPublisher = base_nodeHandle.advertise<dfall_pkg::IntWithHeader>("FlyingAgentClient/CrazyRadioCommand", 1); // > For Flying state commands based on button clicks flyingStateCommandPublisher = base_nodeHandle.advertise<dfall_pkg::IntWithHeader>("FlyingAgentClient/Command", 1); @@ -126,7 +126,7 @@ ConnectStartStopBar::ConnectStartStopBar(QWidget *parent) : batteryLevelSubscriber = base_nodeHandle.subscribe("BatteryMonitor/Level", 1, &ConnectStartStopBar::batteryLevelCallback, this); // > For updating the "flying_state_label" picture - flyingStateSubscriber = base_nodeHandle.subscribe("FlyingAgentClient/flyingState", 1, &ConnectStartStopBar::flyingStateChangedCallback, this); + flyingStateSubscriber = base_nodeHandle.subscribe("FlyingAgentClient/FlyingState", 1, &ConnectStartStopBar::flyingStateChangedCallback, this); } #endif @@ -614,10 +614,10 @@ void ConnectStartStopBar::setAgentIDsToCoordinate(QVector<int> agentIDs , bool s ros::NodeHandle agent_base_nodeHandle(agent_base_namespace.toStdString()); // > Request the current flying state - ros::ServiceClient getCurrentFlyingStateService = agent_base_nodeHandle.serviceClient<dfall_pkg::IntIntService>("FlyingAgentClient/getCurrentFlyingState", false); + ros::ServiceClient getCurrentFlyingStateService = agent_base_nodeHandle.serviceClient<dfall_pkg::IntIntService>("FlyingAgentClient/GetCurrentFlyingState", false); dfall_pkg::IntIntService getFlyingStateCall; getFlyingStateCall.request.data = 0; - getCurrentFlyingStateService.waitForExistence(ros::Duration(2.0)); + getCurrentFlyingStateService.waitForExistence(ros::Duration(0.0)); if(getCurrentFlyingStateService.call(getFlyingStateCall)) { setFlyingState(getFlyingStateCall.response.data); @@ -631,7 +631,7 @@ void ConnectStartStopBar::setAgentIDsToCoordinate(QVector<int> agentIDs , bool s ros::ServiceClient getCurrentCrazyRadioStateService = agent_base_nodeHandle.serviceClient<dfall_pkg::IntIntService>("CrazyRadio/getCurrentCrazyRadioStatus", false); dfall_pkg::IntIntService getCrazyRadioCall; getCrazyRadioCall.request.data = 0; - getCurrentCrazyRadioStateService.waitForExistence(ros::Duration(2.0)); + getCurrentCrazyRadioStateService.waitForExistence(ros::Duration(0.0)); if(getCurrentCrazyRadioStateService.call(getCrazyRadioCall)) { setCrazyRadioStatus(getCrazyRadioCall.response.data); @@ -654,7 +654,7 @@ void ConnectStartStopBar::setAgentIDsToCoordinate(QVector<int> agentIDs , bool s batteryLevelSubscriber = agent_base_nodeHandle.subscribe("BatteryMonitor/Level", 1, &ConnectStartStopBar::batteryLevelCallback, this); // > For updating the "flying_state_label" picture - flyingStateSubscriber = agent_base_nodeHandle.subscribe("FlyingAgentClient/flyingState", 1, &ConnectStartStopBar::flyingStateChangedCallback, this); + flyingStateSubscriber = agent_base_nodeHandle.subscribe("FlyingAgentClient/FlyingState", 1, &ConnectStartStopBar::flyingStateChangedCallback, this); } else { diff --git a/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/src/controllertabs.cpp b/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/src/controllertabs.cpp index f2ca9793..2df07eab 100644 --- a/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/src/controllertabs.cpp +++ b/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/src/controllertabs.cpp @@ -209,7 +209,7 @@ ControllerTabs::ControllerTabs(QWidget *parent) : // Only if this is an agent GUI if (m_type == TYPE_AGENT) { - controllerUsedSubscriber = nodeHandle_for_this_gui.subscribe("FlyingAgentClient/controllerUsed", 1, &ControllerTabs::controllerUsedChangedCallback, this); + controllerUsedSubscriber = nodeHandle_for_this_gui.subscribe("FlyingAgentClient/ControllerUsed", 1, &ControllerTabs::controllerUsedChangedCallback, this); } @@ -218,7 +218,7 @@ ControllerTabs::ControllerTabs(QWidget *parent) : ros::NodeHandle dfall_root_nodeHandle("/dfall"); // > Publisher for the emergency stop button - //emergencyStopPublisher = dfall_root_nodeHandle.advertise<dfall_pkg::IntWithHeader>("emergencyStop", 1); + //emergencyStopPublisher = dfall_root_nodeHandle.advertise<dfall_pkg::IntWithHeader>("EmergencyStop", 1); // > For changes in the database that defines {agentID,cfID,flying zone} links //databaseChangedSubscriber = dfall_root_nodeHandle.subscribe("CentralManagerService/DBChanged", 1, &TopBanner::databaseChangedCallback, this);; @@ -560,10 +560,10 @@ void ControllerTabs::setAgentIDsToCoordinate(QVector<int> agentIDs , bool should // > Request the current instant controller - ros::ServiceClient getInstantControllerService = agent_base_nodeHandle.serviceClient<dfall_pkg::IntIntService>("FlyingAgentClient/getInstantController", false); + ros::ServiceClient getInstantControllerService = agent_base_nodeHandle.serviceClient<dfall_pkg::IntIntService>("FlyingAgentClient/GetInstantController", false); dfall_pkg::IntIntService getInstantControllerCall; getInstantControllerCall.request.data = 0; - getInstantControllerService.waitForExistence(ros::Duration(2.0)); + getInstantControllerService.waitForExistence(ros::Duration(0.1)); if(getInstantControllerService.call(getInstantControllerCall)) { setControllerEnabled(getInstantControllerCall.response.data); @@ -575,7 +575,7 @@ void ControllerTabs::setAgentIDsToCoordinate(QVector<int> agentIDs , bool should // SUBSCRIBERS // > For receiving message that the instant controller was changed - controllerUsedSubscriber = agent_base_nodeHandle.subscribe("FlyingAgentClient/controllerUsed", 1, &ControllerTabs::controllerUsedChangedCallback, this); + controllerUsedSubscriber = agent_base_nodeHandle.subscribe("FlyingAgentClient/ControllerUsed", 1, &ControllerTabs::controllerUsedChangedCallback, this); } else { diff --git a/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/src/coordinatorrow.cpp b/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/src/coordinatorrow.cpp index b6d60f69..eb55555b 100644 --- a/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/src/coordinatorrow.cpp +++ b/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/src/coordinatorrow.cpp @@ -109,7 +109,7 @@ CoordinatorRow::CoordinatorRow(QWidget *parent, int agentID) : // SUBSCRIBERS AND PUBLISHERS: // > For Crazyradio commands based on button clicks - crazyRadioCommandPublisher = base_nodeHandle.advertise<dfall_pkg::IntWithHeader>("FlyingAgentClient/crazyRadioCommand", 1); + crazyRadioCommandPublisher = base_nodeHandle.advertise<dfall_pkg::IntWithHeader>("FlyingAgentClient/CrazyRadioCommand", 1); // > For updating the "rf_status_label" picture crazyRadioStatusSubscriber = base_nodeHandle.subscribe("CrazyRadio/CrazyRadioStatus", 1, &CoordinatorRow::crazyRadioStatusCallback, this); @@ -123,18 +123,18 @@ CoordinatorRow::CoordinatorRow(QWidget *parent, int agentID) : // > For Flying state commands based on button clicks flyingStateCommandPublisher = base_nodeHandle.advertise<dfall_pkg::IntWithHeader>("FlyingAgentClient/Command", 1); // > For updating the "flying_state_label" picture - flyingStateSubscriber = base_nodeHandle.subscribe("FlyingAgentClient/flyingState", 1, &CoordinatorRow::flyingStateChangedCallback, this); + flyingStateSubscriber = base_nodeHandle.subscribe("FlyingAgentClient/FlyingState", 1, &CoordinatorRow::flyingStateChangedCallback, this); // > For changes in the database that defines {agentID,cfID,flying zone} links databaseChangedSubscriber = dfall_root_nodeHandle.subscribe("CentralManagerService/DBChanged", 1, &CoordinatorRow::databaseChangedCallback, this);; centralManagerDatabaseService = dfall_root_nodeHandle.serviceClient<dfall_pkg::CMQuery>("CentralManagerService/Query", false); // > For updating the controller that is currently operating - controllerUsedSubscriber = base_nodeHandle.subscribe("FlyingAgentClient/controllerUsed", 1, &CoordinatorRow::controllerUsedChangedCallback, this); + controllerUsedSubscriber = base_nodeHandle.subscribe("FlyingAgentClient/ControllerUsed", 1, &CoordinatorRow::controllerUsedChangedCallback, this); // > For requesting the current flying state, // this is used only for initialising the icon - getCurrentFlyingStateService = base_nodeHandle.serviceClient<dfall_pkg::IntIntService>("FlyingAgentClient/getCurrentFlyingState", false); + getCurrentFlyingStateService = base_nodeHandle.serviceClient<dfall_pkg::IntIntService>("FlyingAgentClient/GetCurrentFlyingState", false); // > For requesting the current state of the Crazy Radio, // this is used only for initialising the icon @@ -649,7 +649,7 @@ void CoordinatorRow::getCurrentFlyingState() #ifdef CATKIN_MAKE dfall_pkg::IntIntService getFlyingStateCall; getFlyingStateCall.request.data = 0; - getCurrentFlyingStateService.waitForExistence(ros::Duration(2.0)); + getCurrentFlyingStateService.waitForExistence(ros::Duration(0.0)); if(getCurrentFlyingStateService.call(getFlyingStateCall)) { setFlyingState(getFlyingStateCall.response.data); @@ -669,7 +669,7 @@ void CoordinatorRow::getCurrentCrazyRadioState() #ifdef CATKIN_MAKE dfall_pkg::IntIntService getCrazyRadioCall; getCrazyRadioCall.request.data = 0; - getCurrentCrazyRadioStateService.waitForExistence(ros::Duration(2.0)); + getCurrentCrazyRadioStateService.waitForExistence(ros::Duration(0.0)); if(getCurrentCrazyRadioStateService.call(getCrazyRadioCall)) { setCrazyRadioStatus(getCrazyRadioCall.response.data); diff --git a/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/src/csonecontrollertab.cpp b/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/src/csonecontrollertab.cpp index 3f1658e6..7ee1c19a 100644 --- a/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/src/csonecontrollertab.cpp +++ b/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/src/csonecontrollertab.cpp @@ -179,7 +179,7 @@ CsoneControllerTab::CsoneControllerTab(QWidget *parent) : ros::ServiceClient getCurrentSetpointServiceClient = nodeHandle_for_this_gui.serviceClient<dfall_pkg::GetSetpointService>("CsoneControllerService/GetCurrentSetpoint", false); dfall_pkg::GetSetpointService getSetpointCall; getSetpointCall.request.data = 0; - getCurrentSetpointServiceClient.waitForExistence(ros::Duration(2.0)); + getCurrentSetpointServiceClient.waitForExistence(ros::Duration(0.1)); if(getCurrentSetpointServiceClient.call(getSetpointCall)) { setpointChangedCallback(getSetpointCall.response.setpointWithHeader); @@ -194,7 +194,7 @@ CsoneControllerTab::CsoneControllerTab(QWidget *parent) : ros::ServiceClient getCurrentIntegatorStateServiceClient = nodeHandle_for_this_gui.serviceClient<dfall_pkg::IntIntService>("CsoneControllerService/GetCurrentIntegratorState", false); dfall_pkg::IntIntService getIntegrtorStateCall; getIntegrtorStateCall.request.data = 0; - getCurrentIntegatorStateServiceClient.waitForExistence(ros::Duration(2.0)); + getCurrentIntegatorStateServiceClient.waitForExistence(ros::Duration(0.1)); if(getCurrentIntegatorStateServiceClient.call(getIntegrtorStateCall)) { dfall_pkg::IntWithHeader temp_msg; @@ -1100,7 +1100,7 @@ void CsoneControllerTab::setAgentIDsToCoordinate(QVector<int> agentIDs , bool sh ros::ServiceClient getCurrentSetpointServiceClient = agent_base_nodeHandle.serviceClient<dfall_pkg::GetSetpointService>("CsoneControllerService/GetCurrentSetpoint", false); dfall_pkg::GetSetpointService getSetpointCall; getSetpointCall.request.data = 0; - getCurrentSetpointServiceClient.waitForExistence(ros::Duration(2.0)); + getCurrentSetpointServiceClient.waitForExistence(ros::Duration(0.0)); if(getCurrentSetpointServiceClient.call(getSetpointCall)) { setpointChangedCallback(getSetpointCall.response.setpointWithHeader); diff --git a/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/src/defaultcontrollertab.cpp b/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/src/defaultcontrollertab.cpp index 4170841a..7904b231 100644 --- a/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/src/defaultcontrollertab.cpp +++ b/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/src/defaultcontrollertab.cpp @@ -92,7 +92,7 @@ DefaultControllerTab::DefaultControllerTab(QWidget *parent) : ros::ServiceClient getCurrentSetpointServiceClient = nodeHandle_for_this_gui.serviceClient<dfall_pkg::GetSetpointService>("DefaultControllerService/GetCurrentSetpoint", false); dfall_pkg::GetSetpointService getSetpointCall; getSetpointCall.request.data = 0; - getCurrentSetpointServiceClient.waitForExistence(ros::Duration(2.0)); + getCurrentSetpointServiceClient.waitForExistence(ros::Duration(0.1)); if(getCurrentSetpointServiceClient.call(getSetpointCall)) { setpointChangedCallback(getSetpointCall.response.setpointWithHeader); @@ -676,7 +676,7 @@ void DefaultControllerTab::setAgentIDsToCoordinate(QVector<int> agentIDs , bool ros::ServiceClient getCurrentSetpointServiceClient = agent_base_nodeHandle.serviceClient<dfall_pkg::GetSetpointService>("DefaultControllerService/GetCurrentSetpoint", false); dfall_pkg::GetSetpointService getSetpointCall; getSetpointCall.request.data = 0; - getCurrentSetpointServiceClient.waitForExistence(ros::Duration(2.0)); + getCurrentSetpointServiceClient.waitForExistence(ros::Duration(0.0)); if(getCurrentSetpointServiceClient.call(getSetpointCall)) { setpointChangedCallback(getSetpointCall.response.setpointWithHeader); diff --git a/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/src/pickercontrollertab.cpp b/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/src/pickercontrollertab.cpp index f90b07f4..0375c8f6 100644 --- a/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/src/pickercontrollertab.cpp +++ b/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/src/pickercontrollertab.cpp @@ -164,7 +164,7 @@ PickerControllerTab::PickerControllerTab(QWidget *parent) : ros::ServiceClient getCurrentSetpointServiceClient = nodeHandle_for_this_gui.serviceClient<dfall_pkg::GetSetpointService>("PickerControllerService/GetCurrentSetpoint", false); dfall_pkg::GetSetpointService getSetpointCall; getSetpointCall.request.data = 0; - getCurrentSetpointServiceClient.waitForExistence(ros::Duration(2.0)); + getCurrentSetpointServiceClient.waitForExistence(ros::Duration(0.1)); if(getCurrentSetpointServiceClient.call(getSetpointCall)) { setpointChangedCallback(getSetpointCall.response.setpointWithHeader); @@ -1829,7 +1829,7 @@ void PickerControllerTab::setAgentIDsToCoordinate(QVector<int> agentIDs , bool s ros::ServiceClient getCurrentSetpointServiceClient = agent_base_nodeHandle.serviceClient<dfall_pkg::GetSetpointService>("PickerControllerService/GetCurrentSetpoint", false); dfall_pkg::GetSetpointService getSetpointCall; getSetpointCall.request.data = 0; - getCurrentSetpointServiceClient.waitForExistence(ros::Duration(2.0)); + getCurrentSetpointServiceClient.waitForExistence(ros::Duration(0.0)); if(getCurrentSetpointServiceClient.call(getSetpointCall)) { setpointChangedCallback(getSetpointCall.response.setpointWithHeader); diff --git a/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/src/remotecontrollertab.cpp b/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/src/remotecontrollertab.cpp index c897cd93..38385a27 100644 --- a/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/src/remotecontrollertab.cpp +++ b/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/src/remotecontrollertab.cpp @@ -96,7 +96,7 @@ RemoteControllerTab::RemoteControllerTab(QWidget *parent) : // ros::ServiceClient getCurrentSetpointServiceClient = nodeHandle_for_this_gui.serviceClient<dfall_pkg::GetSetpointService>("RemoteControllerService/GetCurrentSetpoint", false); // dfall_pkg::GetSetpointService getSetpointCall; // getSetpointCall.request.data = 0; -// getCurrentSetpointServiceClient.waitForExistence(ros::Duration(2.0)); +// getCurrentSetpointServiceClient.waitForExistence(ros::Duration(0.1)); // if(getCurrentSetpointServiceClient.call(getSetpointCall)) // { // setpointChangedCallback(getSetpointCall.response.setpointWithHeader); @@ -582,7 +582,7 @@ void RemoteControllerTab::setAgentIDsToCoordinate(QVector<int> agentIDs , bool s ros::ServiceClient getCurrentSetpointServiceClient = agent_base_nodeHandle.serviceClient<dfall_pkg::GetSetpointService>("RemoteControllerService/GetCurrentSetpoint", false); dfall_pkg::GetSetpointService getSetpointCall; getSetpointCall.request.data = 0; - getCurrentSetpointServiceClient.waitForExistence(ros::Duration(2.0)); + getCurrentSetpointServiceClient.waitForExistence(ros::Duration(0.0)); if(getCurrentSetpointServiceClient.call(getSetpointCall)) { setpointChangedCallback(getSetpointCall.response.setpointWithHeader); diff --git a/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/src/studentcontrollertab.cpp b/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/src/studentcontrollertab.cpp index 440daaac..64207a97 100644 --- a/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/src/studentcontrollertab.cpp +++ b/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/src/studentcontrollertab.cpp @@ -92,7 +92,7 @@ StudentControllerTab::StudentControllerTab(QWidget *parent) : ros::ServiceClient getCurrentSetpointServiceClient = nodeHandle_for_this_gui.serviceClient<dfall_pkg::GetSetpointService>("StudentControllerService/GetCurrentSetpoint", false); dfall_pkg::GetSetpointService getSetpointCall; getSetpointCall.request.data = 0; - getCurrentSetpointServiceClient.waitForExistence(ros::Duration(2.0)); + getCurrentSetpointServiceClient.waitForExistence(ros::Duration(0.1)); if(getCurrentSetpointServiceClient.call(getSetpointCall)) { setpointChangedCallback(getSetpointCall.response.setpointWithHeader); @@ -713,7 +713,7 @@ void StudentControllerTab::setAgentIDsToCoordinate(QVector<int> agentIDs , bool ros::ServiceClient getCurrentSetpointServiceClient = agent_base_nodeHandle.serviceClient<dfall_pkg::GetSetpointService>("StudentControllerService/GetCurrentSetpoint", false); dfall_pkg::GetSetpointService getSetpointCall; getSetpointCall.request.data = 0; - getCurrentSetpointServiceClient.waitForExistence(ros::Duration(2.0)); + getCurrentSetpointServiceClient.waitForExistence(ros::Duration(0.0)); if(getCurrentSetpointServiceClient.call(getSetpointCall)) { setpointChangedCallback(getSetpointCall.response.setpointWithHeader); diff --git a/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/src/templatecontrollertab.cpp b/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/src/templatecontrollertab.cpp index b0da98f0..24f9fb07 100644 --- a/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/src/templatecontrollertab.cpp +++ b/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/src/templatecontrollertab.cpp @@ -92,7 +92,7 @@ TemplateControllerTab::TemplateControllerTab(QWidget *parent) : ros::ServiceClient getCurrentSetpointServiceClient = nodeHandle_for_this_gui.serviceClient<dfall_pkg::GetSetpointService>("TemplateControllerService/GetCurrentSetpoint", false); dfall_pkg::GetSetpointService getSetpointCall; getSetpointCall.request.data = 0; - getCurrentSetpointServiceClient.waitForExistence(ros::Duration(2.0)); + getCurrentSetpointServiceClient.waitForExistence(ros::Duration(0.1)); if(getCurrentSetpointServiceClient.call(getSetpointCall)) { setpointChangedCallback(getSetpointCall.response.setpointWithHeader); @@ -465,7 +465,7 @@ void TemplateControllerTab::setAgentIDsToCoordinate(QVector<int> agentIDs , bool ros::ServiceClient getCurrentSetpointServiceClient = agent_base_nodeHandle.serviceClient<dfall_pkg::GetSetpointService>("TemplateControllerService/GetCurrentSetpoint", false); dfall_pkg::GetSetpointService getSetpointCall; getSetpointCall.request.data = 0; - getCurrentSetpointServiceClient.waitForExistence(ros::Duration(2.0)); + getCurrentSetpointServiceClient.waitForExistence(ros::Duration(0.0)); if(getCurrentSetpointServiceClient.call(getSetpointCall)) { setpointChangedCallback(getSetpointCall.response.setpointWithHeader); diff --git a/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/src/topbanner.cpp b/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/src/topbanner.cpp index 2b299750..baa47e7b 100644 --- a/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/src/topbanner.cpp +++ b/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/src/topbanner.cpp @@ -93,7 +93,7 @@ TopBanner::TopBanner(QWidget *parent) : ros::NodeHandle dfall_root_nodeHandle("/dfall"); // > Publisher for the emergency stop button - emergencyStopPublisher = dfall_root_nodeHandle.advertise<dfall_pkg::IntWithHeader>("emergencyStop", 1); + emergencyStopPublisher = dfall_root_nodeHandle.advertise<dfall_pkg::IntWithHeader>("EmergencyStop", 1); // > For changes in the database that defines {agentID,cfID,flying zone} links databaseChangedSubscriber = dfall_root_nodeHandle.subscribe("CentralManagerService/DBChanged", 1, &TopBanner::databaseChangedCallback, this);; @@ -184,7 +184,7 @@ void TopBanner::loadCrazyflieContext(int ID_to_request_from_database , int emit_ contextCall.request.studentID = ID_to_request_from_database; //ROS_INFO_STREAM("StudentID:" << m_agentID); - centralManagerDatabaseService.waitForExistence(ros::Duration(2)); + centralManagerDatabaseService.waitForExistence(ros::Duration(2.0)); if(centralManagerDatabaseService.call(contextCall)) { diff --git a/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/src/tuningcontrollertab.cpp b/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/src/tuningcontrollertab.cpp index 6c1bf28f..f6578b32 100644 --- a/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/src/tuningcontrollertab.cpp +++ b/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/src/tuningcontrollertab.cpp @@ -108,7 +108,7 @@ TuningControllerTab::TuningControllerTab(QWidget *parent) : // ros::ServiceClient getCurrentSetpointServiceClient = nodeHandle_for_this_gui.serviceClient<dfall_pkg::GetSetpointService>("TuningControllerService/GetCurrentSetpoint", false); // dfall_pkg::GetSetpointService getSetpointCall; // getSetpointCall.request.data = 0; -// getCurrentSetpointServiceClient.waitForExistence(ros::Duration(2.0)); +// getCurrentSetpointServiceClient.waitForExistence(ros::Duration(0.1)); // if(getCurrentSetpointServiceClient.call(getSetpointCall)) // { // setpointChangedCallback(getSetpointCall.response.setpointWithHeader); @@ -507,7 +507,7 @@ void TuningControllerTab::setAgentIDsToCoordinate(QVector<int> agentIDs , bool s // ros::ServiceClient getCurrentSetpointServiceClient = agent_base_nodeHandle.serviceClient<dfall_pkg::GetSetpointService>("TuningControllerService/GetCurrentSetpoint", false); // dfall_pkg::GetSetpointService getSetpointCall; // getSetpointCall.request.data = 0; -// getCurrentSetpointServiceClient.waitForExistence(ros::Duration(2.0)); +// getCurrentSetpointServiceClient.waitForExistence(ros::Duration(0.0)); // if(getCurrentSetpointServiceClient.call(getSetpointCall)) // { // setpointChangedCallback(getSetpointCall.response.setpointWithHeader); diff --git a/dfall_ws/src/dfall_pkg/GUI_Qt/systemConfigGUI/src/mainguiwindow.cpp b/dfall_ws/src/dfall_pkg/GUI_Qt/systemConfigGUI/src/mainguiwindow.cpp index 1c6173bf..54a48fd3 100755 --- a/dfall_ws/src/dfall_pkg/GUI_Qt/systemConfigGUI/src/mainguiwindow.cpp +++ b/dfall_ws/src/dfall_pkg/GUI_Qt/systemConfigGUI/src/mainguiwindow.cpp @@ -279,7 +279,7 @@ void MainGUIWindow::_init() // CREATE A NODE HANDLE TO THE ROOT OF THE D-FaLL SYSTEM ros::NodeHandle nodeHandle_dfall_root("/dfall"); - emergencyStopPublisher = nodeHandle_dfall_root.advertise<dfall_pkg::IntWithHeader>("emergencyStop", 1); + emergencyStopPublisher = nodeHandle_dfall_root.advertise<dfall_pkg::IntWithHeader>("EmergencyStop", 1); #endif } diff --git a/dfall_ws/src/dfall_pkg/crazyradio/CrazyRadio.py b/dfall_ws/src/dfall_pkg/crazyradio/CrazyRadio.py index 3345d606..25d02f97 100755 --- a/dfall_ws/src/dfall_pkg/crazyradio/CrazyRadio.py +++ b/dfall_ws/src/dfall_pkg/crazyradio/CrazyRadio.py @@ -413,9 +413,9 @@ if __name__ == '__main__': # Subscribe to the commands for when to (dis-)connect the # CrazyRadio connection with the Crazyflie # > For the radio commands from the FlyingAgentClient of this agent - rospy.Subscriber("FlyingAgentClient/crazyRadioCommand", IntWithHeader, cf_client.crazyRadioCommandCallback) + rospy.Subscriber("FlyingAgentClient/CrazyRadioCommand", IntWithHeader, cf_client.crazyRadioCommandCallback) # > For the radio command from the coordinator - rospy.Subscriber("/dfall/coord" + coordID_as_string + "/FlyingAgentClient/crazyRadioCommand", IntWithHeader, cf_client.crazyRadioCommandCallback) + rospy.Subscriber("/dfall/coord" + coordID_as_string + "/FlyingAgentClient/CrazyRadioCommand", IntWithHeader, cf_client.crazyRadioCommandCallback) # Advertise a Serice for the current status diff --git a/dfall_ws/src/dfall_pkg/crazyradio/TestCF.py b/dfall_ws/src/dfall_pkg/crazyradio/TestCF.py index f955bd2c..9598f852 100755 --- a/dfall_ws/src/dfall_pkg/crazyradio/TestCF.py +++ b/dfall_ws/src/dfall_pkg/crazyradio/TestCF.py @@ -223,7 +223,7 @@ if __name__ == '__main__': global cf_client cf_client = CrazyRadioClient() - # rospy.Subscriber("FlyingAgentClient/crazyRadioCommand", Int32, cf_client.crazyRadioCommandCallback) # allows commands from scripts + # rospy.Subscriber("FlyingAgentClient/CrazyRadioCommand", Int32, cf_client.crazyRadioCommandCallback) # allows commands from scripts # time.sleep(1.0) diff --git a/dfall_ws/src/dfall_pkg/src/classes/QuadrotorSimulator.cpp b/dfall_ws/src/dfall_pkg/src/classes/QuadrotorSimulator.cpp index 4741dde7..ff922c8f 100644 --- a/dfall_ws/src/dfall_pkg/src/classes/QuadrotorSimulator.cpp +++ b/dfall_ws/src/dfall_pkg/src/classes/QuadrotorSimulator.cpp @@ -453,7 +453,7 @@ void QuadrotorSimulator::subscribe_to_commanding_agent_id( int commanding_agent_ // Subscribe to the flying state updates of the // commanding agent - flyingStateUpdatesOfCommandingAgentSubscriber = nodeHandle.subscribe("/dfall/agent" + commanding_agent_id_as_string + "/FlyingAgentClient/flyingState", 1, &QuadrotorSimulator::flying_state_update_of_commanding_agent_callback, this ); + flyingStateUpdatesOfCommandingAgentSubscriber = nodeHandle.subscribe("/dfall/agent" + commanding_agent_id_as_string + "/FlyingAgentClient/FlyingState", 1, &QuadrotorSimulator::flying_state_update_of_commanding_agent_callback, this ); // Inform to user ROS_INFO_STREAM("[QUADROTOR SIMULATOR] Quadrotor \"" << this->m_id_string << "\" now subscribed to commands from agent with ID " << commanding_agent_id_as_string); diff --git a/dfall_ws/src/dfall_pkg/src/nodes/BatteryMonitor.cpp b/dfall_ws/src/dfall_pkg/src/nodes/BatteryMonitor.cpp index bf0af445..93f5a305 100644 --- a/dfall_ws/src/dfall_pkg/src/nodes/BatteryMonitor.cpp +++ b/dfall_ws/src/dfall_pkg/src/nodes/BatteryMonitor.cpp @@ -523,7 +523,7 @@ int main(int argc, char* argv[]) ros::Subscriber crazyRadioStatusSubscriber = nodeHandle_to_crazyradio.subscribe("CrazyRadioStatus", 1, crazyRadioStatusCallback); // Subscribe to the flying state of the agent - ros::Subscriber agentOperatingStateSubscriber = nodeHandle_to_FlyingAgentClient.subscribe("flyingState", 1, agentOperatingStateCallback); + ros::Subscriber agentOperatingStateSubscriber = nodeHandle_to_FlyingAgentClient.subscribe("FlyingState", 1, agentOperatingStateCallback); // Initialise the operating state m_agent_operating_state = AGENT_OPERATING_STATE_MOTORS_OFF; diff --git a/dfall_ws/src/dfall_pkg/src/nodes/CrazyRadioEmulator.cpp b/dfall_ws/src/dfall_pkg/src/nodes/CrazyRadioEmulator.cpp index 19fb02d2..f0564252 100644 --- a/dfall_ws/src/dfall_pkg/src/nodes/CrazyRadioEmulator.cpp +++ b/dfall_ws/src/dfall_pkg/src/nodes/CrazyRadioEmulator.cpp @@ -534,9 +534,9 @@ int main(int argc, char* argv[]) // Subscriber to the commands for when to (dis-)connect the // CrazyRadio connection with the Crazyflie // > For the radio commands from the FlyingAgentClient of this agent - ros::Subscriber crazyRadioCommandSubscriber = nodeHandle_to_FlyingAgentClient.subscribe("crazyRadioCommand", 1, crazyRadioCommandCallback); + ros::Subscriber crazyRadioCommandSubscriber = nodeHandle_to_FlyingAgentClient.subscribe("CrazyRadioCommand", 1, crazyRadioCommandCallback); // > For the radio command from the coordinator - ros::Subscriber crazyRadioCommandFromCoordSubscriber = nodeHandle_to_coordinator.subscribe("FlyingAgentClient/crazyRadioCommand", 1, crazyRadioCommandCallback); + ros::Subscriber crazyRadioCommandFromCoordSubscriber = nodeHandle_to_coordinator.subscribe("FlyingAgentClient/CrazyRadioCommand", 1, crazyRadioCommandCallback); // Subscriber for the "Control Commands" received from the Flying Agent Client ros::Subscriber controlCommandsSubscriber = nodeHandle_to_FlyingAgentClient.subscribe("ControlCommand", 1, controlCommandCallback); diff --git a/dfall_ws/src/dfall_pkg/src/nodes/FlyingAgentClient.cpp b/dfall_ws/src/dfall_pkg/src/nodes/FlyingAgentClient.cpp index 488e8e8d..e0613830 100755 --- a/dfall_ws/src/dfall_pkg/src/nodes/FlyingAgentClient.cpp +++ b/dfall_ws/src/dfall_pkg/src/nodes/FlyingAgentClient.cpp @@ -1597,7 +1597,7 @@ int main(int argc, char* argv[]) // EMERGENCY STOP OF THE WHOLE "D-FaLL-System" - ros::Subscriber emergencyStopSubscriber = nodeHandle_dfall_root.subscribe("emergencyStop", 1, emergencyStopReceivedCallback); + ros::Subscriber emergencyStopSubscriber = nodeHandle_dfall_root.subscribe("EmergencyStop", 1, emergencyStopReceivedCallback); // LOCALISATION DATA FROM MOTION CAPTURE SYSTEM @@ -1624,20 +1624,20 @@ int main(int argc, char* argv[]) // i.e., {CONNECT,DISCONNECT} // This topic lets us use the terminal to communicate with // the crazyRadio node even when the GUI is not launched - m_crazyRadioCommandPublisher = nodeHandle.advertise<IntWithHeader>("crazyRadioCommand", 1); + m_crazyRadioCommandPublisher = nodeHandle.advertise<IntWithHeader>("CrazyRadioCommand", 1); // PUBLISHER FOR THE FLYING STATE // Possible states: {MOTORS-OFF,TAKE-OFF,FLYING,LAND} // This topic will publish flying state whenever it changes. - m_flyingStatePublisher = nodeHandle.advertise<std_msgs::Int32>("flyingState", 1); + m_flyingStatePublisher = nodeHandle.advertise<std_msgs::Int32>("FlyingState", 1); // PUBLISHER FOR THE CONTROLLER CURRENTLY IN USE // This publishes the "m_instant_controller" variable // to reflect the controller that is actually called // when motion capture data is received. - m_controllerUsedPublisher = nodeHandle.advertise<std_msgs::Int32>("controllerUsed", 1); + m_controllerUsedPublisher = nodeHandle.advertise<std_msgs::Int32>("ControllerUsed", 1); // SUBSCRIBER FOR CRAZY RADIO STATUS CHANGES @@ -1667,13 +1667,13 @@ int main(int argc, char* argv[]) // SERVICE SERVER FOR OTHERS TO GET THE CURRENT FLYING STATE // Advertise the service that return the "m_flying_state" // variable when called upon - ros::ServiceServer getCurrentFlyingStateService = nodeHandle.advertiseService("getCurrentFlyingState", getCurrentFlyingStateServiceCallback); + ros::ServiceServer getCurrentFlyingStateService = nodeHandle.advertiseService("GetCurrentFlyingState", getCurrentFlyingStateServiceCallback); // SERVICE SERVER FOR OTHERS TO GET THE INSTANT CONTROLLER // Advertise the service that return the "m_instant_controller" // variable when called upon - ros::ServiceServer getInstantControllerService = nodeHandle.advertiseService("getInstantcontroller", getInstantControllerServiceCallback); + ros::ServiceServer getInstantControllerService = nodeHandle.advertiseService("GetInstantController", getInstantControllerServiceCallback); -- GitLab