Commit 8fe7e732 authored by beuchatp's avatar beuchatp
Browse files

Tested to be working and added highlighting the tab label for the controller...

Tested to be working and added highlighting the tab label for the controller that is operating. Next step is to add tabs for the remote and picker controllers, add service for getting the currently operating controller, and switch and PPSClient from the safe controller to the default controller.
parent fe61c0fd
......@@ -11,7 +11,7 @@
#include <ros/package.h>
// Include the standard message types
//#include "std_msgs/Int32.h"
#include "std_msgs/Int32.h"
//#include "std_msgs/Float32.h"
//#include <std_msgs/String.h>
......@@ -90,6 +90,11 @@ private:
// Flag for whether pose data should be emitted, this is
// to save looking through the data when it is unnecessary
bool m_should_search_pose_data_for_object_name = false;
QMutex m_should_search_pose_data_for_object_name_mutex;
// The color for normal and highlighted tabs
QColor m_tab_text_colour_normal;
QColor m_tab_text_colour_highlight;
#ifdef CATKIN_MAKE
......@@ -99,6 +104,8 @@ private:
// SUBSRIBER
// > For the pose data from a motion capture system
ros::Subscriber m_poseDataSubscriber;
// > For the controller that is currently operating
ros::Subscriber controllerUsedSubscriber;
#endif
......@@ -110,6 +117,14 @@ private:
// "controllerUsedSubscriber"
void poseDataReceivedCallback(const d_fall_pps::ViconData& viconData);
void controllerUsedChangedCallback(const std_msgs::Int32& msg);
void setControllerEnabled(int new_controller);
void setAllTabLabelsToNormalColouring();
void setTextColourOfTabLabel(QColor color , QWidget * tab_widget);
// Get the paramters that specify the type and ID
bool getTypeAndIDParameters();
#endif
......
......@@ -8,6 +8,11 @@ ControllerTabs::ControllerTabs(QWidget *parent) :
ui->setupUi(this);
// Specify the color for normal and highlighted tabs
m_tab_text_colour_normal = Qt::black;
m_tab_text_colour_highlight = QColor(0,200,0);
// Initialise the object name as blank
m_object_name_for_emitting_pose_data = "";
......@@ -34,6 +39,11 @@ ControllerTabs::ControllerTabs(QWidget *parent) :
ui->default_controller_tab_widget , &DefaultControllerTab::poseDataUnavailableSlot
);
QObject::connect(
this , &ControllerTabs::poseDataUnavailableSignal ,
ui->student_controller_tab_widget , &StudentControllerTab::poseDataUnavailableSlot
);
// CONNECT TO THE COORDINATOR SIGNAL TO BE ALWAYS UPDATED
// WITH THE LIST OF AGENT IDs TO COORDINATE
......@@ -45,6 +55,11 @@ ControllerTabs::ControllerTabs(QWidget *parent) :
ui->default_controller_tab_widget , &DefaultControllerTab::setAgentIDsToCoordinate
);
QObject::connect(
this , &ControllerTabs::agentIDsToCoordinateChanged ,
ui->student_controller_tab_widget , &StudentControllerTab::setAgentIDsToCoordinate
);
......@@ -68,6 +83,7 @@ ControllerTabs::ControllerTabs(QWidget *parent) :
}
// CREATE A NODE HANDLE TO THIS GUI
ros::NodeHandle nodeHandle_for_this_gui(this_namespace);
......@@ -77,6 +93,14 @@ ControllerTabs::ControllerTabs(QWidget *parent) :
// CREATE THE SUBSCRIBER TO THE MOTION CAPTURE DATA
m_poseDataSubscriber = nodeHandle_dfall_root.subscribe("ViconDataPublisher/ViconData", 100, &ControllerTabs::poseDataReceivedCallback, this);
// CREATE THE SUBSCRIBER TO THE CONTROLLER THAT IS CURRENTLY OPERATING
// Only if this is an agent GUI
if (m_type == TYPE_AGENT)
{
controllerUsedSubscriber = nodeHandle_for_this_gui.subscribe("PPSClient/controllerUsed", 1, &ControllerTabs::controllerUsedChangedCallback, this);
}
#endif
}
......@@ -136,6 +160,7 @@ void ControllerTabs::showHideController_safe_changed()
void ControllerTabs::setObjectNameForDisplayingPoseData( QString object_name )
{
m_should_search_pose_data_for_object_name_mutex.lock();
if (object_name.isEmpty())
{
// Set the class variable accordingly
......@@ -160,6 +185,7 @@ void ControllerTabs::setObjectNameForDisplayingPoseData( QString object_name )
ROS_INFO_STREAM("[CONTROLLER TABS GUI] now emitting data for object named: " << m_object_name_for_emitting_pose_data );
#endif
}
m_should_search_pose_data_for_object_name_mutex.unlock();
}
......@@ -168,6 +194,7 @@ void ControllerTabs::setObjectNameForDisplayingPoseData( QString object_name )
// "controllerUsedSubscriber"
void ControllerTabs::poseDataReceivedCallback(const d_fall_pps::ViconData& viconData)
{
m_should_search_pose_data_for_object_name_mutex.lock();
if (m_should_search_pose_data_for_object_name)
{
for(std::vector<d_fall_pps::CrazyflieData>::const_iterator it = viconData.crazyflies.begin(); it != viconData.crazyflies.end(); ++it)
......@@ -188,6 +215,7 @@ void ControllerTabs::poseDataReceivedCallback(const d_fall_pps::ViconData& vicon
}
}
}
m_should_search_pose_data_for_object_name_mutex.unlock();
}
#endif
......@@ -195,6 +223,102 @@ void ControllerTabs::poseDataReceivedCallback(const d_fall_pps::ViconData& vicon
// ----------------------------------------------------------------------------------
// CCCC OOO N N TTTTT RRRR OOO L L EEEEE RRRR
// C O O NN N T R R O O L L E R R
// C O O N N N T RRRR O O L L EEE RRRR
// C O O N NN T R R O O L L E R R
// CCCC OOO N N T R R OOO LLLLL LLLLL EEEEE R R
//
// EEEEE N N A BBBB L EEEEE DDDD
// E NN N A A B B L E D D
// EEE N N N A A BBBB L EEE D D
// E N NN AAAAA B B L E D D
// EEEEE N N A A BBBB LLLLL EEEEE DDDD
// ----------------------------------------------------------------------------------
#ifdef CATKIN_MAKE
// > For the controller currently operating, received on "controllerUsedSubscriber"
void ControllerTabs::controllerUsedChangedCallback(const std_msgs::Int32& msg)
{
//ROS_INFO_STEAM("[COORDINATOR ROW GUI] Controller Used Changed Callback called for agentID = " << m_agentID);
setControllerEnabled(msg.data);
}
#endif
void ControllerTabs::setControllerEnabled(int new_controller)
{
// First set everything back to normal colouring
setAllTabLabelsToNormalColouring();
// Now switch to highlight the tab corresponding to
// the enable controller
switch(new_controller)
{
case SAFE_CONTROLLER:
{
//ui->controller_enabled_label->setText("Safe");
break;
}
case DEMO_CONTROLLER:
{
//ui->controller_enabled_label->setText("Demo");
break;
}
case STUDENT_CONTROLLER:
{
setTextColourOfTabLabel( m_tab_text_colour_highlight , ui->student_tab );
break;
}
case MPC_CONTROLLER:
{
//ui->controller_enabled_label->setText("MPC");
break;
}
case REMOTE_CONTROLLER:
{
//ui->controller_enabled_label->setText("Remote");
break;
}
case TUNING_CONTROLLER:
{
//ui->controller_enabled_label->setText("Tuning");
break;
}
default:
{
//ui->controller_enabled_label->setText("Unknown");
break;
}
}
}
void ControllerTabs::setAllTabLabelsToNormalColouring()
{
setTextColourOfTabLabel( m_tab_text_colour_normal , ui->default_tab );
setTextColourOfTabLabel( m_tab_text_colour_normal , ui->student_tab );
setTextColourOfTabLabel( m_tab_text_colour_normal , ui->picker_tab );
setTextColourOfTabLabel( m_tab_text_colour_normal , ui->safe_tab );
}
void ControllerTabs::setTextColourOfTabLabel(QColor color , QWidget * tab_widget)
{
// Get the current index of the tab
int current_index_of_tab = ui->controller_tabs_widget->indexOf(tab_widget);
// Onlz apply the colour is the tab is found
if (current_index_of_tab >= 0)
{
ui->controller_tabs_widget->tabBar()->setTabTextColor(current_index_of_tab, color);
}
}
// ----------------------------------------------------------------------------------
// A GGGG EEEEE N N TTTTT III DDDD SSSS
// A A G E NN N T I D D S
......@@ -206,8 +330,53 @@ void ControllerTabs::poseDataReceivedCallback(const d_fall_pps::ViconData& vicon
void ControllerTabs::setAgentIDsToCoordinate(QVector<int> agentIDs , bool shouldCoordinateAll)
{
// Simply pass on the signal to the tabs
// Pass on the signal to the tabs
emit agentIDsToCoordinateChanged( agentIDs , shouldCoordinateAll );
// Lock the mutex
m_agentIDs_toCoordinate_mutex.lock();
// Add the "coordinate all" flag
m_shouldCoordinateAll = shouldCoordinateAll;
// Clear the previous list of agent IDs
m_vector_of_agentIDs_toCoordinate.clear();
// Copy across the agent IDs, if necessary
if (!shouldCoordinateAll)
{
for ( int irow = 0 ; irow < agentIDs.length() ; irow++ )
{
m_vector_of_agentIDs_toCoordinate.push_back( agentIDs[irow] );
}
}
// Unlock the mutex
m_agentIDs_toCoordinate_mutex.unlock();
#ifdef CATKIN_MAKE
// If there is only one agent to coordinate,
// then subscribe to the relevant data
if (agentIDs.length() == 1)
{
// // > Create the appropriate node handle
QString agent_base_namespace = "/dfall/agent" + QString::number(agentIDs[0]).rightJustified(3, '0');
ros::NodeHandle agent_base_nodeHandle(agent_base_namespace.toStdString());
// SUBSCRIBERS
// > For receiving message that the setpoint was changed
controllerUsedSubscriber = agent_base_nodeHandle.subscribe("PPSClient/controllerUsed", 1, &ControllerTabs::controllerUsedChangedCallback, this);
}
else
{
// Unsubscribe
controllerUsedSubscriber.shutdown();
// Set all tabs to be normal colours
setAllTabLabelsToNormalColouring();
}
#endif
}
......
......@@ -46,6 +46,26 @@ DefaultControllerTab::DefaultControllerTab(QWidget *parent) :
setpointChangedSubscriber = nodeHandle_for_this_gui.subscribe("DefaultControllerService/SetpointChanged", 1, &DefaultControllerTab::setpointChangedCallback, this);
}
// GET THE CURRENT SETPOINT
// Only if this is an agent GUI
if (m_type == TYPE_AGENT)
{
// > Request the current setpoint
ros::ServiceClient getCurrentSetpointServiceClient = nodeHandle_for_this_gui.serviceClient<d_fall_pps::GetSetpointService>("DefaultControllerService/GetCurrentSetpoint", false);
d_fall_pps::GetSetpointService getSetpointCall;
getSetpointCall.request.data = 0;
getCurrentSetpointServiceClient.waitForExistence(ros::Duration(2.0));
if(getCurrentSetpointServiceClient.call(getSetpointCall))
{
setpointChangedCallback(getSetpointCall.response.setpointWithHeader);
}
else
{
// Inform the user
ROS_INFO("[DEFAULT CONTROLLER GUI] Failed to get setpoint from controller using the \"GetCurrentSetpoint\" service");
}
}
#endif
}
......@@ -555,7 +575,7 @@ void DefaultControllerTab::setAgentIDsToCoordinate(QVector<int> agentIDs , bool
QString agent_base_namespace = "/dfall/agent" + QString::number(agentIDs[0]).rightJustified(3, '0');
ros::NodeHandle agent_base_nodeHandle(agent_base_namespace.toStdString());
// // > Request the current setpoint
// > Request the current setpoint
ros::ServiceClient getCurrentSetpointServiceClient = agent_base_nodeHandle.serviceClient<d_fall_pps::GetSetpointService>("DefaultControllerService/GetCurrentSetpoint", false);
d_fall_pps::GetSetpointService getSetpointCall;
getSetpointCall.request.data = 0;
......
......@@ -46,9 +46,29 @@ StudentControllerTab::StudentControllerTab(QWidget *parent) :
setpointChangedSubscriber = nodeHandle_for_this_gui.subscribe("StudentControllerService/SetpointChanged", 1, &StudentControllerTab::setpointChangedCallback, this);
}
// CREATE THE CUSTOM BUTTON PRESSED PUBLISHED
// CREATE THE CUSTOM BUTTON PRESSED PUBLISHER
customButtonPublisher = nodeHandle_for_this_gui.advertise<d_fall_pps::CustomButtonWithHeader>("StudentControllerService/CustomButtonPressed", 1);
// GET THE CURRENT SETPOINT
// Only if this is an agent GUI
if (m_type == TYPE_AGENT)
{
// > Request the current setpoint
ros::ServiceClient getCurrentSetpointServiceClient = nodeHandle_for_this_gui.serviceClient<d_fall_pps::GetSetpointService>("StudentControllerService/GetCurrentSetpoint", false);
d_fall_pps::GetSetpointService getSetpointCall;
getSetpointCall.request.data = 0;
getCurrentSetpointServiceClient.waitForExistence(ros::Duration(2.0));
if(getCurrentSetpointServiceClient.call(getSetpointCall))
{
setpointChangedCallback(getSetpointCall.response.setpointWithHeader);
}
else
{
// Inform the user
ROS_INFO("[STUDENT CONTROLLER GUI] Failed to get setpoint from controller using the \"GetCurrentSetpoint\" service");
}
}
#endif
}
......@@ -106,52 +126,34 @@ void StudentControllerTab::on_custom_button_1_clicked()
{
#ifdef CATKIN_MAKE
publish_custom_button_command(1,ui->lineEdit_custom_1);
// // Initialise the message as a local variable
// d_fall_pps::CustomButtonWithHeader msg;
// // Fill the header of the message
// fillCustomButtonMessageHeader( msg );
// // Fill in the button index
// msg.button_index = 1;
// // Get the line edit data, as a float if possible
// bool isFloat = false;
// float lineEdit_as_float = (ui->lineEdit_custom_1->text()).toFloat(isFloat);
// // Fill in the data
// if (isFloat)
// msg.float_data = lineEdit_as_float;
// else
// msg.string_data = (ui->lineEdit_custom_1->text()).toStdString();
// // Publish the setpoint
// this->customButtonPublisher.publish(msg);
// // Inform the user about the change
// ROS_INFO("[STUDENT CONTROLLER TAB GUI] button 1 clicked.");
#endif
}
void StudentControllerTab::on_custom_button_2_clicked()
{
#ifdef CATKIN_MAKE
ROS_INFO("[STUDENT CONTROLLER TAB GUI] button 2 clicked.");
publish_custom_button_command(2,ui->lineEdit_custom_2);
#endif
}
void StudentControllerTab::on_custom_button_3_clicked()
{
#ifdef CATKIN_MAKE
ROS_INFO("[STUDENT CONTROLLER TAB GUI] button 3 clicked.");
publish_custom_button_command(3,ui->lineEdit_custom_3);
#endif
}
void StudentControllerTab::on_custom_button_4_clicked()
{
#ifdef CATKIN_MAKE
ROS_INFO("[STUDENT CONTROLLER TAB GUI] button 4 clicked.");
publish_custom_button_command(4,ui->lineEdit_custom_4);
#endif
}
void StudentControllerTab::on_custom_button_5_clicked()
{
#ifdef CATKIN_MAKE
ROS_INFO("[STUDENT CONTROLLER TAB GUI] button 5 clicked.");
publish_custom_button_command(5,ui->lineEdit_custom_5);
#endif
}
......
......@@ -933,10 +933,10 @@ int main(int argc, char* argv[]) {
// Same again but instead for changes requested by the coordinator.
// For this we need to first create a node handle to the coordinator:
std::string namespace_to_coordinator;
constructNamespaceForCoordinator( m_coordID, namespace_to_coordinator );
ros::NodeHandle nodeHandle_to_coordinator(namespace_to_coordinator);
// And now we can instantiate the subscriber:
std::string namespace_to_coordinator;
constructNamespaceForCoordinator( m_coordID, namespace_to_coordinator );
ros::NodeHandle nodeHandle_to_coordinator(namespace_to_coordinator);
// And now we can instantiate the subscriber:
ros::Subscriber requestSetpointChangeSubscriber_from_coord = nodeHandle_to_coordinator.subscribe("DefaultControllerService/RequestSetpointChange", 1, requestSetpointChangeCallback);
// Instantiate the class variable "m_setpointChangedPublisher" to
......@@ -979,7 +979,7 @@ int main(int argc, char* argv[]) {
// to the name space of this node, i.e., "d_fall_pps" as specified by the line:
// "using namespace d_fall_pps;"
// in the "DEFINES" section at the top of this file.
ros::NodeHandle namespace_nodeHandle(ros::this_node::getNamespace());
//ros::NodeHandle namespace_nodeHandle(ros::this_node::getNamespace());
// Print out some information to the user.
ROS_INFO("[DEFAULT CONTROLLER] Service ready :-)");
......
......@@ -656,7 +656,7 @@ void customCommandReceivedCallback(const CustomButtonWithHeader& commandReceived
case 1:
{
// Let the user know that this part of the code was triggered
ROS_INFO("[STUDENT CONTROLLER] Button 1 received in controller.");
ROS_INFO_STREAM("[STUDENT CONTROLLER] Button 1 received in controller, with message.float_data = " << float_data );
// Code here to respond to custom button 1
break;
......@@ -666,7 +666,7 @@ void customCommandReceivedCallback(const CustomButtonWithHeader& commandReceived
case 2:
{
// Let the user know that this part of the code was triggered
ROS_INFO("[STUDENT CONTROLLER] Button 2 received in controller.");
ROS_INFO_STREAM("[STUDENT CONTROLLER] Button 2 received in controller, with message.float_data = " << float_data );
// Code here to respond to custom button 2
break;
......@@ -987,6 +987,14 @@ int main(int argc, char* argv[]) {
// by the user.
ros::Subscriber requestSetpointChangeSubscriber = nodeHandle.subscribe("RequestSetpointChange", 1, requestSetpointChangeCallback);
// Same again but instead for changes requested by the coordinator.
// For this we need to first create a node handle to the coordinator:
std::string namespace_to_coordinator;
constructNamespaceForCoordinator( m_coordID, namespace_to_coordinator );
ros::NodeHandle nodeHandle_to_coordinator(namespace_to_coordinator);
// And now we can instantiate the subscriber:
ros::Subscriber requestSetpointChangeSubscriber_from_coord = nodeHandle_to_coordinator.subscribe("StudentControllerService/RequestSetpointChange", 1, requestSetpointChangeCallback);
// Instantiate the class variable "m_setpointChangedPublisher" to
// be a "ros::Publisher". This variable advertises under the name
// "SetpointChanged" and is a message with the structure defined
......@@ -1023,11 +1031,19 @@ int main(int argc, char* argv[]) {
// and the message received is passed as an input argument to the callback function.
ros::Subscriber customCommandReceivedSubscriber = nodeHandle.subscribe("CustomButtonPressed", 1, customCommandReceivedCallback);
// Same again but instead for changes requested by the coordinator.
// For this we need to first create a node handle to the coordinator:
//std::string namespace_to_coordinator;
//constructNamespaceForCoordinator( m_coordID, namespace_to_coordinator );
//ros::NodeHandle nodeHandle_to_coordinator(namespace_to_coordinator);
// And now we can instantiate the subscriber:
ros::Subscriber customCommandReceivedSubscriber_from_coord = nodeHandle_to_coordinator.subscribe("StudentControllerService/CustomButtonPressed", 1, customCommandReceivedCallback);
// Create a "ros::NodeHandle" type local variable "namespace_nodeHandle" that points
// to the name space of this node, i.e., "d_fall_pps" as specified by the line:
// "using namespace d_fall_pps;"
// in the "DEFINES" section at the top of this file.
ros::NodeHandle namespace_nodeHandle(ros::this_node::getNamespace());
//ros::NodeHandle namespace_nodeHandle(ros::this_node::getNamespace());
// Print out some information to the user.
ROS_INFO("[STUDENT CONTROLLER] Service ready :-)");
......
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