From 9f5669fc0b2b66130c7e21ab0548087cb55b90d8 Mon Sep 17 00:00:00 2001
From: Paul Beuchat <beuchatp@control.ee.ethz.ch>
Date: Mon, 23 Apr 2018 12:26:06 +0200
Subject: [PATCH] Added connection from Custom Buttons to Demo controller
---
.../GUI_Qt/studentGUI/src/MainWindow.cpp | 6 +-
.../include/DemoControllerService.h | 5 +-
.../d_fall_pps/src/DemoControllerService.cpp | 72 ++++++++++++++++++-
3 files changed, 78 insertions(+), 5 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 d86a3dd0..78ad4d1f 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
@@ -685,7 +685,7 @@ void MainWindow::on_customButton_1_clicked()
msg_custom_button.command_code = 0;
this->PPSClientStudentCustomButtonPublisher.publish(msg_custom_button);
- ROS_INFO("Custom button 1 pressed");
+ ROS_INFO("Custom button 1 pressed in GUI");
}
void MainWindow::on_customButton_2_clicked()
@@ -694,7 +694,7 @@ void MainWindow::on_customButton_2_clicked()
msg_custom_button.button_index = 2;
msg_custom_button.command_code = 0;
this->PPSClientStudentCustomButtonPublisher.publish(msg_custom_button);
- ROS_INFO("Custom button 2 pressed");
+ ROS_INFO("Custom button 2 pressed in GUI");
}
void MainWindow::on_customButton_3_clicked()
@@ -703,7 +703,7 @@ void MainWindow::on_customButton_3_clicked()
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");
+ ROS_INFO("Custom button 3 pressed in GUI");
}
Setpoint MainWindow::correctSetpointBox(Setpoint setpoint, CrazyflieContext context)
diff --git a/pps_ws/src/d_fall_pps/include/DemoControllerService.h b/pps_ws/src/d_fall_pps/include/DemoControllerService.h
index b788a8b5..953c0522 100644
--- a/pps_ws/src/d_fall_pps/include/DemoControllerService.h
+++ b/pps_ws/src/d_fall_pps/include/DemoControllerService.h
@@ -56,7 +56,7 @@
#include "d_fall_pps/ControlCommand.h"
#include "d_fall_pps/Controller.h"
#include "d_fall_pps/DebugMsg.h"
-//#include "d_fall_pps/DemoControllerYAML.h"
+#include "d_fall_pps/CustomButton.h"
#include <std_msgs/Int32.h>
@@ -409,6 +409,9 @@ float computeMotorPolyBackward(float thrust);
void setpointCallback(const Setpoint& newSetpoint);
void xyz_yaw_to_follow_callback(const Setpoint& newSetpoint);
+// CUSTOM COMMAND RECEIVED CALLBACK
+void customCommandReceivedCallback(const CustomButton& commandReceived);
+
// PUBLISH THE CURRENT X,Y,Z, AND YAW
void publish_current_xyz_yaw(float x, float y, float z, float yaw);
diff --git a/pps_ws/src/d_fall_pps/src/DemoControllerService.cpp b/pps_ws/src/d_fall_pps/src/DemoControllerService.cpp
index 5e351d37..38f548f0 100644
--- a/pps_ws/src/d_fall_pps/src/DemoControllerService.cpp
+++ b/pps_ws/src/d_fall_pps/src/DemoControllerService.cpp
@@ -1144,7 +1144,6 @@ float computeMotorPolyBackward(float thrust)
-
// ----------------------------------------------------------------------------------
// N N EEEEE W W SSSS EEEEE TTTTT PPPP OOO III N N TTTTT
// NN N E W W S E T P P O O I NN N T
@@ -1188,6 +1187,71 @@ void xyz_yaw_to_follow_callback(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("Custom 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("Custom 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("Custom 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("A custom command was received in the controller but not recognised, message.button_index = " << custom_button_index << ", and message.command_code = " << custom_command_code );
+ break;
+ }
+ }
+}
+
+
// ************************************************************************************************
// PPPP U U BBBB L III SSSS H H X X Y Y ZZZZZ Y Y A W W
@@ -1693,6 +1757,12 @@ int main(int argc, char* argv[]) {
// have the structure defined in the file "Setpoint.msg" (located in the "msg" folder).
my_current_xyz_yaw_publisher = nodeHandle.advertise<Setpoint>("/" + std::to_string(my_agentID) + "/my_current_xyz_yaw_topic", 1);
+ // Instantiate the local variable "customCommandSubscriber" to be a "ros::Subscriber"
+ // 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);
+
// Print out some information to the user.
ROS_INFO("DemoControllerService ready");
--
GitLab