Commit 36579e13 authored by beuchatp's avatar beuchatp
Browse files

Added custom buttons to student controller, needs testing

parent a17f4449
......@@ -158,9 +158,13 @@ private slots:
void on_customButton_1_clicked();
void on_customButton_2_clicked();
void on_customButton_3_clicked();
void on_demoButton_1_clicked();
void on_demoButton_2_clicked();
void on_demoButton_3_clicked();
void on_studentButton_1_clicked();
void on_studentButton_2_clicked();
void on_studentButton_3_clicked();
// Buttons within the REMOTE controller tab
void on_remote_subscribe_button_clicked();
......@@ -246,8 +250,8 @@ private:
ros::Publisher tuningHeadingGainPublisher;
ros::Publisher PPSClientStudentCustomButtonPublisher;
ros::Publisher demoCustomButtonPublisher;
ros::Publisher studentCustomButtonPublisher;
ros::Subscriber DBChangedSubscriber;
......
......@@ -68,12 +68,14 @@ MainWindow::MainWindow(int argc, char **argv, QWidget *parent) :
// SUBSCRIBERS AND PUBLISHERS:
// > For the Demo Controller SETPOINTS
demoSetpointPublisher = nodeHandle.advertise<Setpoint>("DemoControllerService/Setpoint", 1);
demoSetpointSubscriber = nodeHandle.subscribe("DemoControllerService/Setpoint", 1, &MainWindow::demoSetpointCallback, this);
// > For the Student Controller SETPOINTS
studentSetpointPublisher = nodeHandle.advertise<Setpoint>("StudentControllerService/Setpoint", 1);
studentSetpointSubscriber = nodeHandle.subscribe("StudentControllerService/Setpoint", 1, &MainWindow::studentSetpointCallback, this);
// > For the Demo Controller SETPOINTS and CUSTOM COMMANDS
demoSetpointPublisher = nodeHandle.advertise<Setpoint>("DemoControllerService/Setpoint", 1);
demoSetpointSubscriber = nodeHandle.subscribe("DemoControllerService/Setpoint", 1, &MainWindow::demoSetpointCallback, this);
demoCustomButtonPublisher = nodeHandle.advertise<CustomButton>("DemoControllerService/GUIButton", 1);
// > For the Student Controller SETPOINTS and CUSTOM COMMANDS
studentSetpointPublisher = nodeHandle.advertise<Setpoint>("StudentControllerService/Setpoint", 1);
studentSetpointSubscriber = nodeHandle.subscribe("StudentControllerService/Setpoint", 1, &MainWindow::studentSetpointCallback, this);
studentCustomButtonPublisher = nodeHandle.advertise<CustomButton>("StudentControllerService/GUIButton", 1);
// > For the MPC Controller SETPOINTS
mpcSetpointPublisher = nodeHandle.advertise<Setpoint>("MpcControllerService/Setpoint", 1);
mpcSetpointSubscriber = nodeHandle.subscribe("MpcControllerService/Setpoint", 1, &MainWindow::mpcSetpointCallback, this);
......@@ -121,9 +123,7 @@ MainWindow::MainWindow(int argc, char **argv, QWidget *parent) :
//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);
PPSClientStudentCustomButtonPublisher = nh_PPSClient.advertise<CustomButton>("StudentCustomButton", 1);
PPSClientCommandPublisher = nh_PPSClient.advertise<std_msgs::Int32>("Command", 1);
// > For publishing a message that requests the
......@@ -1081,33 +1081,66 @@ void MainWindow::on_enable_tuning_controller_clicked()
// # Custom command buttons
void MainWindow::on_customButton_1_clicked()
// # Custom command buttons - FOR DEMO CONTROLLER
void MainWindow::on_demoButton_1_clicked()
{
CustomButton msg_custom_button;
msg_custom_button.button_index = 1;
msg_custom_button.command_code = 0;
this->PPSClientStudentCustomButtonPublisher.publish(msg_custom_button);
this->demoCustomButtonPublisher.publish(msg_custom_button);
ROS_INFO("Custom button 1 pressed in GUI");
ROS_INFO("Demo button 1 pressed in GUI");
}
void MainWindow::on_customButton_2_clicked()
void MainWindow::on_demoButton_2_clicked()
{
CustomButton msg_custom_button;
msg_custom_button.button_index = 2;
msg_custom_button.command_code = 0;
this->PPSClientStudentCustomButtonPublisher.publish(msg_custom_button);
ROS_INFO("Custom button 2 pressed in GUI");
this->demoCustomButtonPublisher.publish(msg_custom_button);
ROS_INFO("Demo button 2 pressed in GUI");
}
void MainWindow::on_customButton_3_clicked()
{
CustomButton msg_custom_button;
msg_custom_button.button_index = 3;
msg_custom_button.command_code = (ui->custom_command_3->text()).toFloat();
this->PPSClientStudentCustomButtonPublisher.publish(msg_custom_button);
ROS_INFO("Custom button 3 pressed in GUI");
msg_custom_button.command_code = (ui->demoField_3->text()).toFloat();
this->demoCustomButtonPublisher.publish(msg_custom_button);
ROS_INFO("Demo button 3 pressed in GUI");
}
// # Custom command buttons - FOR STUDENT CONTROLLER
void MainWindow::on_studentButton_1_clicked()
{
CustomButton msg_custom_button;
msg_custom_button.button_index = 1;
msg_custom_button.command_code = 0;
this->studentCustomButtonPublisher.publish(msg_custom_button);
ROS_INFO("Student button 1 pressed in GUI");
}
void MainWindow::on_studentButton_2_clicked()
{
CustomButton msg_custom_button;
msg_custom_button.button_index = 2;
msg_custom_button.command_code = 0;
this->studentCustomButtonPublisher.publish(msg_custom_button);
ROS_INFO("Student button 2 pressed in GUI");
}
void MainWindow::on_studentButton_3_clicked()
{
CustomButton msg_custom_button;
msg_custom_button.button_index = 3;
msg_custom_button.command_code = (ui->demoField_3->text()).toFloat();
this->studentCustomButtonPublisher.publish(msg_custom_button);
ROS_INFO("Student button 3 pressed in GUI");
}
......
......@@ -6,8 +6,8 @@
<rect>
<x>0</x>
<y>0</y>
<width>1333</width>
<height>721</height>
<width>1500</width>
<height>1000</height>
</rect>
</property>
<property name="sizePolicy">
......@@ -369,7 +369,7 @@
</font>
</property>
<property name="currentIndex">
<number>0</number>
<number>2</number>
</property>
<property name="usesScrollButtons">
<bool>true</bool>
......@@ -1700,7 +1700,7 @@
<number>6</number>
</property>
<item>
<widget class="QPushButton" name="customButton_1">
<widget class="QPushButton" name="demoButton_1">
<property name="sizePolicy">
<sizepolicy hsizetype="MinimumExpanding" vsizetype="Fixed">
<horstretch>0</horstretch>
......@@ -1713,12 +1713,12 @@
</font>
</property>
<property name="text">
<string>Custom Command 1</string>
<string>Command 1</string>
</property>
</widget>
</item>
<item>
<widget class="QPushButton" name="customButton_2">
<widget class="QPushButton" name="demoButton_2">
<property name="sizePolicy">
<sizepolicy hsizetype="MinimumExpanding" vsizetype="Fixed">
<horstretch>0</horstretch>
......@@ -1731,12 +1731,12 @@
</font>
</property>
<property name="text">
<string>Custom Command 2</string>
<string>Command 2</string>
</property>
</widget>
</item>
<item>
<widget class="QPushButton" name="customButton_3">
<widget class="QPushButton" name="demoButton_3">
<property name="sizePolicy">
<sizepolicy hsizetype="MinimumExpanding" vsizetype="Fixed">
<horstretch>0</horstretch>
......@@ -1749,12 +1749,12 @@
</font>
</property>
<property name="text">
<string>Custom Command 3</string>
<string>Command 3</string>
</property>
</widget>
</item>
<item>
<widget class="QLineEdit" name="custom_command_3">
<widget class="QLineEdit" name="demoField_3">
<property name="font">
<font>
<pointsize>14</pointsize>
......@@ -1787,6 +1787,49 @@
<property name="bottomMargin">
<number>6</number>
</property>
<item row="15" column="1">
<widget class="QPushButton" name="studentButton_2">
<property name="sizePolicy">
<sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
<horstretch>0</horstretch>
<verstretch>0</verstretch>
</sizepolicy>
</property>
<property name="minimumSize">
<size>
<width>0</width>
<height>40</height>
</size>
</property>
<property name="maximumSize">
<size>
<width>16777215</width>
<height>50</height>
</size>
</property>
<property name="text">
<string>Commnd 2</string>
</property>
</widget>
</item>
<item row="11" column="0">
<widget class="QLabel" name="label_25">
<property name="sizePolicy">
<sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
<horstretch>0</horstretch>
<verstretch>0</verstretch>
</sizepolicy>
</property>
<property name="font">
<font>
<pointsize>14</pointsize>
</font>
</property>
<property name="text">
<string>yaw [deg] =</string>
</property>
</widget>
</item>
<item row="1" column="2">
<widget class="QLabel" name="label_27">
<property name="sizePolicy">
......@@ -1818,7 +1861,7 @@
</property>
<property name="font">
<font>
<pointsize>14</pointsize>
<pointsize>16</pointsize>
</font>
</property>
<property name="text">
......@@ -1883,24 +1926,6 @@
</property>
</widget>
</item>
<item row="11" column="0">
<widget class="QLabel" name="label_25">
<property name="sizePolicy">
<sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
<horstretch>0</horstretch>
<verstretch>0</verstretch>
</sizepolicy>
</property>
<property name="font">
<font>
<pointsize>14</pointsize>
</font>
</property>
<property name="text">
<string>yaw [deg] =</string>
</property>
</widget>
</item>
<item row="9" column="0">
<widget class="QLabel" name="label_24">
<property name="sizePolicy">
......@@ -2069,6 +2094,78 @@
</property>
</widget>
</item>
<item row="15" column="0">
<widget class="QPushButton" name="studentButton_1">
<property name="sizePolicy">
<sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
<horstretch>0</horstretch>
<verstretch>0</verstretch>
</sizepolicy>
</property>
<property name="minimumSize">
<size>
<width>0</width>
<height>40</height>
</size>
</property>
<property name="maximumSize">
<size>
<width>16777215</width>
<height>50</height>
</size>
</property>
<property name="text">
<string>Command 1</string>
</property>
</widget>
</item>
<item row="15" column="2">
<widget class="QPushButton" name="studentButton_3">
<property name="sizePolicy">
<sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
<horstretch>0</horstretch>
<verstretch>0</verstretch>
</sizepolicy>
</property>
<property name="minimumSize">
<size>
<width>0</width>
<height>40</height>
</size>
</property>
<property name="maximumSize">
<size>
<width>16777215</width>
<height>50</height>
</size>
</property>
<property name="text">
<string>Command 3</string>
</property>
</widget>
</item>
<item row="15" column="3">
<widget class="QLineEdit" name="studentField_3">
<property name="sizePolicy">
<sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
<horstretch>0</horstretch>
<verstretch>0</verstretch>
</sizepolicy>
</property>
<property name="minimumSize">
<size>
<width>0</width>
<height>40</height>
</size>
</property>
<property name="maximumSize">
<size>
<width>16777215</width>
<height>50</height>
</size>
</property>
</widget>
</item>
</layout>
</item>
</layout>
......@@ -3700,8 +3797,8 @@
<rect>
<x>0</x>
<y>0</y>
<width>1333</width>
<height>25</height>
<width>1500</width>
<height>37</height>
</rect>
</property>
</widget>
......
......@@ -207,6 +207,9 @@ float computeMotorPolyBackward(float thrust);
// SETPOINT CHANGE CALLBACK
void setpointCallback(const Setpoint& newSetpoint);
// CUSTOM COMMAND RECEIVED CALLBACK
void customCommandReceivedCallback(const CustomButton& commandReceived);
// LOAD PARAMETERS
float getParameterFloat(ros::NodeHandle& nodeHandle, std::string name);
void getParameterFloatVector(ros::NodeHandle& nodeHandle, std::string name, std::vector<float>& val, int length);
......
......@@ -1925,7 +1925,7 @@ int main(int argc, char* argv[]) {
// type variable that subscribes to the "StudentCustomButton" topic and calls the class
// function "customCommandReceivedCallback" each time a messaged is received on this topic
// and the message received is passed as an input argument to the callback function.
ros::Subscriber customCommandReceivedSubscriber = PPSClient_nodeHandle.subscribe("StudentCustomButton", 1, customCommandReceivedCallback);
ros::Subscriber customCommandReceivedSubscriber = nodeHandle.subscribe("GUIButton", 1, customCommandReceivedCallback);
// Print out some information to the user.
ROS_INFO("[DEMO CONTROLLER] Service ready :-)");
......
......@@ -524,6 +524,75 @@ void setpointCallback(const Setpoint& newSetpoint)
// ----------------------------------------------------------------------------------
// CCCC U U SSSS TTTTT OOO M M
// C U U S T O O MM MM
// C U U SSS T O O M M M
// C U U S T O O M M
// CCCC UUU SSSS T OOO M M
//
// CCCC OOO M M M M A N N DDDD
// C O O MM MM MM MM A A NN N D D
// C O O M M M M M M A A N N N D D
// C O O M M M M AAAAA N NN D D
// CCCC OOO M M M M A A N N DDDD
// ----------------------------------------------------------------------------------
// CUSTOM COMMAND RECEIVED CALLBACK
void customCommandReceivedCallback(const CustomButton& commandReceived)
{
// Extract the data from the message
int custom_button_index = commandReceived.button_index;
float custom_command_code = commandReceived.command_code;
// Switch between the button pressed
switch(custom_button_index)
{
// > FOR CUSTOM BUTTON 1
case 1:
{
// Let the user know that this part of the code was triggered
ROS_INFO("[STUDENT CONTROLLER] Button 1 received in controller.");
// Code here to respond to custom button 1
break;
}
// > FOR CUSTOM BUTTON 2
case 2:
{
// Let the user know that this part of the code was triggered
ROS_INFO("[STUDENT CONTROLLER] Button 2 received in controller.");
// Code here to respond to custom button 2
break;
}
// > FOR CUSTOM BUTTON 3
case 3:
{
// Let the user know that this part of the code was triggered
ROS_INFO_STREAM("[STUDENT CONTROLLER] Button 3 received in controller, with command code:" << custom_command_code );
// Code here to respond to custom button 3
break;
}
default:
{
// Let the user know that the command was not recognised
ROS_INFO_STREAM("[DEMO CONTROLLER] A button clicked command was received in the controller but not recognised, message.button_index = " << custom_button_index << ", and message.command_code = " << custom_command_code );
break;
}
}
}
// ----------------------------------------------------------------------------------
// L OOO A DDDD
// L O O A A D D
......@@ -830,6 +899,12 @@ int main(int argc, char* argv[]) {
// of this service the "calculateControlOutput" function is called.
ros::ServiceServer service = nodeHandle.advertiseService("StudentController", calculateControlOutput);
// Instantiate the local variable "customCommandSubscriber" to be a "ros::Subscriber"
// type variable that subscribes to the "GUIButton" topic and calls the class
// function "customCommandReceivedCallback" each time a messaged is received on this topic
// and the message received is passed as an input argument to the callback function.
ros::Subscriber customCommandReceivedSubscriber = nodeHandle.subscribe("GUIButton", 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;"
......
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