diff --git a/pps_ws/src/d_fall_pps/CMakeLists.txt b/pps_ws/src/d_fall_pps/CMakeLists.txt
index d08a3c4c2a7b92ec5093c96f9d3fe91c47231c9b..51ad5d3dbcec868a9675cddb83479e0c35cfb3ff 100755
--- a/pps_ws/src/d_fall_pps/CMakeLists.txt
+++ b/pps_ws/src/d_fall_pps/CMakeLists.txt
@@ -128,10 +128,13 @@ set(SRC_HDRS_QOBJECT_FLYING_AGENT_GUI
${FLYING_AGENT_GUI_LIB_PATH_INC}/controllertabs.h
${FLYING_AGENT_GUI_LIB_PATH_INC}/coordinator.h
${FLYING_AGENT_GUI_LIB_PATH_INC}/coordinatorrow.h
+ ${FLYING_AGENT_GUI_LIB_PATH_INC}/defaultcontrollertab.h
${FLYING_AGENT_GUI_LIB_PATH_INC}/enablecontrollerloadyamlbar.h
${FLYING_AGENT_GUI_LIB_PATH_INC}/mainwindow.h
+ ${FLYING_AGENT_GUI_LIB_PATH_INC}/pickercontrollertab.h
${FLYING_AGENT_GUI_LIB_PATH_INC}/rosNodeThread_for_flyingAgentGUI.h
${FLYING_AGENT_GUI_LIB_PATH_INC}/safecontrollertab.h
+ ${FLYING_AGENT_GUI_LIB_PATH_INC}/studentcontrollertab.h
${FLYING_AGENT_GUI_LIB_PATH_INC}/topbanner.h
)
# Flying Agent GUI -- wrap UI file and QOBJECT files
@@ -140,9 +143,12 @@ qt5_wrap_ui(UIS_HDRS_FLYING_AGENT_GUI
${FLYING_AGENT_GUI_LIB_PATH_FORMS}/controllertabs.ui
${FLYING_AGENT_GUI_LIB_PATH_FORMS}/coordinator.ui
${FLYING_AGENT_GUI_LIB_PATH_FORMS}/coordinatorrow.ui
+ ${FLYING_AGENT_GUI_LIB_PATH_FORMS}/defaultcontrollertab.ui
${FLYING_AGENT_GUI_LIB_PATH_FORMS}/enablecontrollerloadyamlbar.ui
${FLYING_AGENT_GUI_LIB_PATH_FORMS}/mainwindow.ui
+ ${FLYING_AGENT_GUI_LIB_PATH_FORMS}/pickercontrollertab.ui
${FLYING_AGENT_GUI_LIB_PATH_FORMS}/safecontrollertab.ui
+ ${FLYING_AGENT_GUI_LIB_PATH_FORMS}/studentcontrollertab.ui
${FLYING_AGENT_GUI_LIB_PATH_FORMS}/topbanner.ui
)
qt5_wrap_cpp(SRC_MOC_HDRS_FLYING_AGENT_GUI ${SRC_HDRS_QOBJECT_FLYING_AGENT_GUI})
@@ -317,6 +323,7 @@ add_executable(StudentControllerService src/nodes/StudentControllerService.cpp)
add_executable(MpcControllerService src/nodes/MpcControllerService.cpp)
add_executable(RemoteControllerService src/nodes/RemoteControllerService.cpp)
add_executable(TuningControllerService src/nodes/TuningControllerService.cpp)
+add_executable(PickerControllerService src/nodes/PickerControllerService.cpp)
add_executable(CentralManagerService src/nodes/CentralManagerService.cpp src/CrazyflieIO.cpp)
add_executable(ParameterService src/nodes/ParameterService.cpp)
@@ -361,8 +368,11 @@ set(MY_CPP_SOURCES_FLYING_AGENT_GUI # compilation of sources
${FLYING_AGENT_GUI_LIB_PATH_SRC}/controllertabs.cpp
${FLYING_AGENT_GUI_LIB_PATH_SRC}/coordinator.cpp
${FLYING_AGENT_GUI_LIB_PATH_SRC}/coordinatorrow.cpp
+ ${FLYING_AGENT_GUI_LIB_PATH_SRC}/defaultcontrollertab.cpp
${FLYING_AGENT_GUI_LIB_PATH_SRC}/enablecontrollerloadyamlbar.cpp
+ ${FLYING_AGENT_GUI_LIB_PATH_SRC}/pickercontrollertab.cpp
${FLYING_AGENT_GUI_LIB_PATH_SRC}/safecontrollertab.cpp
+ ${FLYING_AGENT_GUI_LIB_PATH_SRC}/studentcontrollertab.cpp
${FLYING_AGENT_GUI_LIB_PATH_SRC}/topbanner.cpp
)
@@ -396,6 +406,7 @@ add_dependencies(StudentControllerService d_fall_pps_generate_messages_cpp ${cat
add_dependencies(MpcControllerService d_fall_pps_generate_messages_cpp ${catkin_EXPORTED_TARGETS})
add_dependencies(RemoteControllerService d_fall_pps_generate_messages_cpp ${catkin_EXPORTED_TARGETS})
add_dependencies(TuningControllerService d_fall_pps_generate_messages_cpp ${catkin_EXPORTED_TARGETS})
+add_dependencies(PickerControllerService d_fall_pps_generate_messages_cpp ${catkin_EXPORTED_TARGETS})
add_dependencies(CentralManagerService d_fall_pps_generate_messages_cpp ${catkin_EXPORTED_TARGETS})
add_dependencies(ParameterService d_fall_pps_generate_messages_cpp ${catkin_EXPORTED_TARGETS})
@@ -449,6 +460,7 @@ else()
endif()
target_link_libraries(RemoteControllerService ${catkin_LIBRARIES})
target_link_libraries(TuningControllerService ${catkin_LIBRARIES})
+target_link_libraries(PickerControllerService ${catkin_LIBRARIES})
target_link_libraries(CentralManagerService ${catkin_LIBRARIES})
target_link_libraries(ParameterService ${catkin_LIBRARIES})
diff --git a/pps_ws/src/d_fall_pps/GUI_Qt/studentGUI/include/MainWindow.h b/pps_ws/src/d_fall_pps/GUI_Qt/studentGUI/include/MainWindow.h
index 8b1a0bec068ea359cb2bb93ba86c1304e78d6fd1..56c3fefb7c2c12c4e0c78bd2060c4f04c12c0729 100644
--- a/pps_ws/src/d_fall_pps/GUI_Qt/studentGUI/include/MainWindow.h
+++ b/pps_ws/src/d_fall_pps/GUI_Qt/studentGUI/include/MainWindow.h
@@ -323,12 +323,13 @@ private:
// > For the PICKER CONTROLLER
- ros::Publisher pickerButtonPressedPublisher;
- ros::Publisher pickerZSetpointPublisher;
- ros::Publisher pickerYawSetpointPublisher;
- ros::Publisher pickerMassPublisher;
- ros::Publisher pickerXAdjustmentPublisher;
- ros::Publisher pickerYAdjustmentPublisher;
+ ros::Publisher pickerButtonPressedPublisher;
+ ros::Publisher pickerZSetpointPublisher;
+ ros::Publisher pickerYawSetpointPublisher;
+ ros::Publisher pickerMassPublisher;
+ ros::Publisher pickerXAdjustmentPublisher;
+ ros::Publisher pickerYAdjustmentPublisher;
+ ros::Publisher pickerSetpointPublisher;
ros::Subscriber pickerSetpointSubscriber;
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 e27c1887dbed43407fd7dfeb8aba2f7d1d53a2c2..59d5a0aa562f25d6000ff2f0c6e975285205386c 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
@@ -105,8 +105,16 @@ MainWindow::MainWindow(int argc, char **argv, QWidget *parent) :
pickerMassPublisher = nodeHandle.advertise<std_msgs::Float32>("PickerControllerService/Mass", 1);
pickerXAdjustmentPublisher = nodeHandle.advertise<std_msgs::Float32>("PickerControllerService/XAdjustment", 1);
pickerYAdjustmentPublisher = nodeHandle.advertise<std_msgs::Float32>("PickerControllerService/YAdjustment", 1);
+ pickerSetpointPublisher = nodeHandle.advertise<Setpoint>("PickerControllerService/Setpoint", 1);
pickerSetpointSubscriber = nodeHandle.subscribe("PickerControllerService/Setpoint", 1, &MainWindow::pickerSetpointCallback, this);
+ // SET ALL SLIDERS AND DIALS TO DEFAULT VALUES
+ ui->picker_z_slider->setValue( 40 );
+ ui->picker_mass_slider->setValue( 29 );
+ ui->picker_yaw_dial->setValue( 0 );
+ ui->picker_x_slider->setValue( 0 );
+ ui->picker_y_slider->setValue( 0 );
+
// subscribers
@@ -399,7 +407,7 @@ void MainWindow::pickerSetpointCallback(const Setpoint& newSetpoint)
{
m_picker_setpoint = newSetpoint;
// here we get the new setpoint, need to update it in GUI
- ui->picker_z_slider->setValue( int(newSetpoint.z) );
+ ui->picker_z_slider->setValue( int(newSetpoint.z*100.0) );
ui->picker_yaw_dial->setValue( int(newSetpoint.yaw * RAD2DEG) );
}
@@ -1786,7 +1794,8 @@ void MainWindow::on_tuning_slider_heading_valueChanged(int value)
// > FOR THE BUTTONS
-void MainWindow::send_picker_button_clicked_message(int button_index){
+void MainWindow::send_picker_button_clicked_message(int button_index)
+{
// Initialise the message
std_msgs::Int32 msg;
// Set the msg data
@@ -1825,22 +1834,22 @@ void MainWindow::on_picker_disconnect_button_clicked()
// Call the function that sends the message
send_picker_button_clicked_message(PICKER_BUTTON_DISCONNECT);
}
-void on_picker_1_button_clicked()
+void MainWindow::on_picker_1_button_clicked()
{
// Call the function that sends the message
send_picker_button_clicked_message(PICKER_BUTTON_1);
}
-void on_picker_2_button_clicked()
+void MainWindow::on_picker_2_button_clicked()
{
// Call the function that sends the message
- send_picker_button_clicked_message(PICKER_BUTTON_1);
+ send_picker_button_clicked_message(PICKER_BUTTON_2);
}
-void on_picker_3_button_clicked()
+void MainWindow::on_picker_3_button_clicked()
{
// Call the function that sends the message
send_picker_button_clicked_message(PICKER_BUTTON_3);
}
-void on_picker_4_button_clicked()
+void MainWindow::on_picker_4_button_clicked()
{
// Call the function that sends the message
send_picker_button_clicked_message(PICKER_BUTTON_4);
@@ -1850,7 +1859,7 @@ void on_picker_4_button_clicked()
// > FOR THE SLIDERS AND DIAL
-void on_picker_x_slider_valueChanged(int value)
+void MainWindow::on_picker_x_slider_valueChanged(int value)
{
// Initialise the message
std_msgs::Float32 msg;
@@ -1859,7 +1868,7 @@ void on_picker_x_slider_valueChanged(int value)
// Publish the message
this->pickerXAdjustmentPublisher.publish(msg);
}
-void on_picker_y_slider_valueChanged(int value)
+void MainWindow::on_picker_y_slider_valueChanged(int value)
{
// Initialise the message
std_msgs::Float32 msg;
@@ -1868,7 +1877,7 @@ void on_picker_y_slider_valueChanged(int value)
// Publish the message
this->pickerYAdjustmentPublisher.publish(msg);
}
-void on_picker_z_slider_valueChanged(int value)
+void MainWindow::on_picker_z_slider_valueChanged(int value)
{
// Initialise the message
std_msgs::Float32 msg;
@@ -1877,7 +1886,7 @@ void on_picker_z_slider_valueChanged(int value)
// Publish the message
this->pickerZSetpointPublisher.publish(msg);
}
-void on_picker_mass_slider_valueChanged(int value)
+void MainWindow::on_picker_mass_slider_valueChanged(int value)
{
// Initialise the message
std_msgs::Float32 msg;
@@ -1886,7 +1895,7 @@ void on_picker_mass_slider_valueChanged(int value)
// Publish the message
this->pickerMassPublisher.publish(msg);
}
-void on_picker_yaw_dial_valueChanged(int value)
+void MainWindow::on_picker_yaw_dial_valueChanged(int value)
{
// Initialise the message
std_msgs::Float32 msg;
diff --git a/pps_ws/src/d_fall_pps/GUI_Qt/studentGUI/src/MainWindow.ui b/pps_ws/src/d_fall_pps/GUI_Qt/studentGUI/src/MainWindow.ui
index afdcb81150d459a398c585b124483582c64fc4e0..130272d412b8ee0ba5f35fc813c6c63963cfa17f 100644
--- a/pps_ws/src/d_fall_pps/GUI_Qt/studentGUI/src/MainWindow.ui
+++ b/pps_ws/src/d_fall_pps/GUI_Qt/studentGUI/src/MainWindow.ui
@@ -1129,7 +1129,7 @@
</widget>
</item>
<item>
- <widget class="QPushButton" name="picker_connect_button_2">
+ <widget class="QPushButton" name="picker_disconnect_button">
<property name="text">
<string>Disconnect</string>
</property>
@@ -4652,7 +4652,7 @@
<x>0</x>
<y>0</y>
<width>1500</width>
- <height>40</height>
+ <height>25</height>
</rect>
</property>
</widget>
diff --git a/pps_ws/src/d_fall_pps/include/nodes/PickerControllerService.h b/pps_ws/src/d_fall_pps/include/nodes/PickerControllerService.h
index e9f0e283327d7daebe673db332feb8523ccd8a83..676bae219f7e2df7d168608a0ddaaa854bfd7a2d 100644
--- a/pps_ws/src/d_fall_pps/include/nodes/PickerControllerService.h
+++ b/pps_ws/src/d_fall_pps/include/nodes/PickerControllerService.h
@@ -62,6 +62,7 @@
#include "nodes/ParameterServiceDefinitions.h"
#include <std_msgs/Int32.h>
+#include <std_msgs/Float32.h>
@@ -171,12 +172,12 @@ float m_mass_H_grams;
float m_mass_total_grams;
// (x,y) coordinates of the pickup location
-float m_pickup_coordinates_xy[2];
+std::vector<float> m_pickup_coordinates_xy(2);
// (x,y) coordinates of the drop off location
-float m_dropoff_coordinates_xy_for_E[2];
-float m_dropoff_coordinates_xy_for_T[2];
-float m_dropoff_coordinates_xy_for_H[2];
+std::vector<float> m_dropoff_coordinates_xy_for_E(2);
+std::vector<float> m_dropoff_coordinates_xy_for_T(2);
+std::vector<float> m_dropoff_coordinates_xy_for_H(2);
// > The setpoints for (x,y,z) position and yaw angle, in that order
float m_setpoint[4] = {0.0,0.0,0.4,0.0};
@@ -399,7 +400,7 @@ void setpointCallback(const Setpoint& newSetpoint);
void xyz_yaw_to_follow_callback(const Setpoint& newSetpoint);
// CUSTOM COMMAND RECEIVED CALLBACK
-void customCommandReceivedCallback(const CustomButton& commandReceived);
+//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/launch/Agent.launch b/pps_ws/src/d_fall_pps/launch/Agent.launch
index d7712739df539bf4b13781487b9a8c961d39a09c..eb8c339ac53843e8855edf4366545255468fb320 100755
--- a/pps_ws/src/d_fall_pps/launch/Agent.launch
+++ b/pps_ws/src/d_fall_pps/launch/Agent.launch
@@ -89,6 +89,15 @@
>
</node>
+ <!-- PICKER CONTROLLER -->
+ <node
+ pkg = "d_fall_pps"
+ name = "PickerControllerService"
+ output = "screen"
+ type = "PickerControllerService"
+ >
+ </node>
+
<!-- PARAMETER SERVICE -->
<node
pkg = "d_fall_pps"
diff --git a/pps_ws/src/d_fall_pps/param/Crazyflie.db b/pps_ws/src/d_fall_pps/param/Crazyflie.db
index 1543fc531957685254699294e839d02f14b6376d..c65ad4e7b1648ed8258bca92bbb537e574e9ceb3 100644
--- a/pps_ws/src/d_fall_pps/param/Crazyflie.db
+++ b/pps_ws/src/d_fall_pps/param/Crazyflie.db
@@ -1,3 +1 @@
-1,PPS_CF05,0/32/2M/E7E7E7E705,0,-1.97017,-1.2375,-0.2,2.11699,0.911783,1.2
-2,PPS_CF06,0/40/2M/E7E7E7E706,0,-1.97017,-1.2375,-0.2,2.11699,0.911783,1.2
-3,PPS_CF07,0/48/2M/E7E7E7E707,0,-1.97017,-1.2375,-0.2,2.11699,0.911783,1.2
+4,PPS_CF03,0/16/2M/E7E7E7E703,0,0.345924,-1.31629,-0.2,4.30616,1.15886,1.2
diff --git a/pps_ws/src/d_fall_pps/src/nodes/PickerControllerService.cpp b/pps_ws/src/d_fall_pps/src/nodes/PickerControllerService.cpp
index 80334bf4bb0abc0b2ba152fef1c3990de407a4a7..ec9cbf4d8971df66d6222fde6d78fc85168836e1 100644
--- a/pps_ws/src/d_fall_pps/src/nodes/PickerControllerService.cpp
+++ b/pps_ws/src/d_fall_pps/src/nodes/PickerControllerService.cpp
@@ -641,7 +641,6 @@ void construct_and_publish_debug_message(Controller::Request &request, Controlle
// debugMsg.value_1 = thrustAdjustment;
// ......................
// debugMsg.value_10 = your_variable_name;
- debugMsg.value_1 = angleResponseTest_prev_pitchAngle;
// Publish the "debugMsg"
debugPublisher.publish(debugMsg);
@@ -1053,7 +1052,7 @@ void fetchYamlParameters(ros::NodeHandle& nodeHandle)
// DEBUGGING: Print out one of the parameters that was loaded
- ROS_INFO_STREAM("[PICKER CONTROLLER] DEBUGGING: the fetched PickerController/mass_CF = " << cf_mass);
+ ROS_INFO_STREAM("[PICKER CONTROLLER] DEBUGGING: the fetched PickerController/mass_CF = " << m_mass_CF_grams);
// Call the function that computes details an values that are needed from these
// parameters loaded above
@@ -1289,7 +1288,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 = nodeHandle.subscribe("GUIButton", 1, customCommandReceivedCallback);
+ //ros::Subscriber buttonPressedCallbackSubscriber = nodeHandle.subscribe("GUIButton", 1, customCommandReceivedCallback);