diff --git a/pps_ws/src/d_fall_pps/CMakeLists.txt b/pps_ws/src/d_fall_pps/CMakeLists.txt
index 51ad5d3dbcec868a9675cddb83479e0c35cfb3ff..222de5fae7dff4b3a96b1eecb8125a261bbd8b9c 100755
--- a/pps_ws/src/d_fall_pps/CMakeLists.txt
+++ b/pps_ws/src/d_fall_pps/CMakeLists.txt
@@ -202,6 +202,7 @@ add_message_files(
CrazyflieContext.msg
AreaBounds.msg
Setpoint.msg
+ SetpointV2.msg
CrazyflieEntry.msg
CrazyflieDB.msg
#----------------------------------------------------------------------
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 2695bc3d042637fe48e211e93329feed0bcd08e1..48d6bda3aa802985938d78d4121a880e5d842dd5 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
@@ -44,6 +44,7 @@
#include "d_fall_pps/CrazyflieContext.h"
#include "d_fall_pps/CrazyflieData.h"
#include "d_fall_pps/Setpoint.h"
+#include "d_fall_pps/SetpointV2.h"
#include "d_fall_pps/ViconSubscribeObjectName.h"
@@ -121,12 +122,12 @@
// FOR WHICH BUTTON WAS PRESSED IN THE PICKER CONTOLLER
#define PICKER_BUTTON_GOTOSTART 1
-#define PICKER_BUTTON_CONNECT 2
+#define PICKER_BUTTON_ATTACH 2
#define PICKER_BUTTON_PICKUP 3
#define PICKER_BUTTON_GOTOEND 4
#define PICKER_BUTTON_PUTDOWN 5
#define PICKER_BUTTON_SQUAT 6
-#define PICKER_BUTTON_DISCONNECT 7
+#define PICKER_BUTTON_JUMP 7
#define PICKER_BUTTON_1 11
#define PICKER_BUTTON_2 12
@@ -217,12 +218,12 @@ private slots:
// Interations with the PICKER controller tab
// > For the buttons
void on_picker_gotostart_button_clicked();
- void on_picker_connect_button_clicked();
+ void on_picker_attach_button_clicked();
void on_picker_pickup_button_clicked();
void on_picker_gotoend_button_clicked();
void on_picker_putdown_button_clicked();
void on_picker_squat_button_clicked();
- void on_picker_disconnect_button_clicked();
+ void on_picker_jump_button_clicked();
void on_picker_1_button_clicked();
void on_picker_2_button_clicked();
void on_picker_3_button_clicked();
@@ -335,6 +336,10 @@ private:
ros::Subscriber pickerSetpointSubscriber;
ros::Subscriber pickerSetpointToGUISubscriber;
+ ros::Publisher pickerButtonPressedWithSetpointPublisher;
+
+ bool shouldSendWithSetpoint_for_pickerButtons = true;
+
@@ -378,6 +383,7 @@ private:
// > For actually sending the button message
void send_picker_button_clicked_message(int button_index);
+ void send_picker_button_clicked_message_with_setpoint(const SetpointV2& setpointV2_to_send);
void DBChangedCallback(const std_msgs::Int32& msg);
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 c51e7a6382c18b503a521aa24ab9d4e2013ff5d4..b39565f4fbc5ce4233125bd3e3f2c0c04b732293 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
@@ -109,6 +109,8 @@ MainWindow::MainWindow(int argc, char **argv, QWidget *parent) :
pickerSetpointSubscriber = nodeHandle.subscribe("PickerControllerService/Setpoint", 1, &MainWindow::pickerSetpointCallback, this);
pickerSetpointToGUISubscriber = nodeHandle.subscribe("PickerControllerService/SetpointToGUI", 1, &MainWindow::pickerSetpointCallback, this);
+ pickerButtonPressedWithSetpointPublisher = nodeHandle.advertise<SetpointV2>("PickerControllerService/ButtonPressedWithSetpoint", 1);
+
// SET ALL SLIDERS AND DIALS TO DEFAULT VALUES
ui->picker_z_slider->setValue( 40 );
ui->picker_mass_slider->setValue( 29 );
@@ -1805,60 +1807,269 @@ void MainWindow::send_picker_button_clicked_message(int button_index)
this->pickerButtonPressedPublisher.publish(msg);
}
+void MainWindow::send_picker_button_clicked_message_with_setpoint(const SetpointV2& setpointV2_to_send)
+{
+ // Publish the message
+ this->pickerButtonPressedPublisher.publish(setpointV2_to_send);
+}
+
void MainWindow::on_picker_gotostart_button_clicked()
{
- // Call the function that sends the message
- send_picker_button_clicked_message(PICKER_BUTTON_GOTOSTART);
+ if (!shouldSendWithSetpoint_for_pickerButtons)
+ {
+ // Call the function that sends the message
+ send_picker_button_clicked_message(PICKER_BUTTON_GOTOSTART);
+ }
+ else
+ {
+ // Construct the setpoint
+ SetpointV2 msg_setpointV2;
+ msg_setpointV2.buttonID = PICKER_BUTTON_GOTOSTART;
+ msg_setpointV2.isChecked = ui->picker_gotostart_checkbox->isChecked();
+ msg_setpointV2.x = 0.0;
+ msg_setpointV2.y = 0.0;
+ msg_setpointV2.z = 0.0;
+ msg_setpointV2.yaw = 0.0;
+ // Get the (x,y,z) coordinates from the GUI
+ if(!ui->picker_gotostart_x->text().isEmpty())
+ msg_setpointV2.x = (ui->picker_gotostart_x->text()).toFloat();
+ if(!ui->picker_gotostart_y->text().isEmpty())
+ msg_setpointV2.y = (ui->picker_gotostart_y->text()).toFloat();
+ if(!ui->picker_gotostart_z->text().isEmpty())
+ msg_setpointV2.z = (ui->picker_gotostart_z->text()).toFloat();
+ // Call the function that sends the message
+ send_picker_button_clicked_message_with_setpoint(msg_setpointV2);
+ }
}
-void MainWindow::on_picker_connect_button_clicked()
+void MainWindow::on_picker_attach_button_clicked()
{
- // Call the function that sends the message
- send_picker_button_clicked_message(PICKER_BUTTON_CONNECT);
+ if (!shouldSendWithSetpoint_for_pickerButtons)
+ {
+ // Call the function that sends the message
+ send_picker_button_clicked_message(PICKER_BUTTON_ATTACH);
+ }
+ else
+ {
+ // Construct the setpoint
+ SetpointV2 msg_setpointV2;
+ msg_setpointV2.buttonID = PICKER_BUTTON_ATTACH;
+ msg_setpointV2.isChecked = ui->picker_attach_checkbox->isChecked();;
+ msg_setpointV2.x = 0.0;
+ msg_setpointV2.y = 0.0;
+ msg_setpointV2.z = 0.0;
+ msg_setpointV2.yaw = 0.0;
+ // Get the z coordinates from the GUI
+ if(!ui->picker_attach_z->text().isEmpty())
+ msg_setpointV2.z = (ui->picker_attach_z->text()).toFloat();
+ // Call the function that sends the message
+ send_picker_button_clicked_message_with_setpoint(msg_setpointV2);
+ }
}
void MainWindow::on_picker_pickup_button_clicked()
{
- // Call the function that sends the message
- send_picker_button_clicked_message(PICKER_BUTTON_PICKUP);
+ if (!shouldSendWithSetpoint_for_pickerButtons)
+ {
+ // Call the function that sends the message
+ send_picker_button_clicked_message(PICKER_BUTTON_PICKUP);
+ }
+ else
+ {
+ // Construct the setpoint
+ SetpointV2 msg_setpointV2;
+ msg_setpointV2.buttonID = PICKER_BUTTON_PICKUP;
+ msg_setpointV2.isChecked = ui->picker_pickup_checkbox->isChecked();;
+ msg_setpointV2.x = 0.0;
+ msg_setpointV2.y = 0.0;
+ msg_setpointV2.z = 0.0;
+ msg_setpointV2.yaw = 0.0;
+ // Get the z coordinates from the GUI
+ if(!ui->picker_pickup_z->text().isEmpty())
+ msg_setpointV2.z = (ui->picker_pickup_z->text()).toFloat();
+ // Call the function that sends the message
+ send_picker_button_clicked_message_with_setpoint(msg_setpointV2);
+ }
}
void MainWindow::on_picker_gotoend_button_clicked()
{
- // Call the function that sends the message
- send_picker_button_clicked_message(PICKER_BUTTON_GOTOEND);
+ if (!shouldSendWithSetpoint_for_pickerButtons)
+ {
+ // Call the function that sends the message
+ send_picker_button_clicked_message(PICKER_BUTTON_GOTOEND);
+ }
+ else
+ {
+ // Construct the setpoint
+ SetpointV2 msg_setpointV2;
+ msg_setpointV2.buttonID = PICKER_BUTTON_GOTOEND;
+ msg_setpointV2.isChecked = ui->picker_gotoend_checkbox->isChecked();;
+ msg_setpointV2.x = 0.0;
+ msg_setpointV2.y = 0.0;
+ msg_setpointV2.z = 0.0;
+ msg_setpointV2.yaw = 0.0;
+ // Get the (x,y) coordinates from the GUI
+ if(!ui->picker_gotoend_x->text().isEmpty())
+ msg_setpointV2.x = (ui->picker_gotoend_x->text()).toFloat();
+ if(!ui->picker_gotoend_y->text().isEmpty())
+ msg_setpointV2.y = (ui->picker_gotoend_y->text()).toFloat();
+ // Call the function that sends the message
+ send_picker_button_clicked_message_with_setpoint(msg_setpointV2);
+ }
}
void MainWindow::on_picker_putdown_button_clicked()
{
- // Call the function that sends the message
- send_picker_button_clicked_message(PICKER_BUTTON_PUTDOWN);
+ if (!shouldSendWithSetpoint_for_pickerButtons)
+ {
+ // Call the function that sends the message
+ send_picker_button_clicked_message(PICKER_BUTTON_PUTDOWN);
+ }
+ else
+ {
+ // Construct the setpoint
+ SetpointV2 msg_setpointV2;
+ msg_setpointV2.buttonID = PICKER_BUTTON_PUTDOWN;
+ msg_setpointV2.isChecked = ui->picker_putdown_checkbox->isChecked();;
+ msg_setpointV2.x = 0.0;
+ msg_setpointV2.y = 0.0;
+ msg_setpointV2.z = 0.0;
+ msg_setpointV2.yaw = 0.0;
+ // Get the z coordinates from the GUI
+ if(!ui->picker_putdown_z->text().isEmpty())
+ msg_setpointV2.z = (ui->picker_putdown_z->text()).toFloat();
+ // Call the function that sends the message
+ send_picker_button_clicked_message_with_setpoint(msg_setpointV2);
+ }
}
void MainWindow::on_picker_squat_button_clicked()
{
- // Call the function that sends the message
- send_picker_button_clicked_message(PICKER_BUTTON_SQUAT);
+ if (!shouldSendWithSetpoint_for_pickerButtons)
+ {
+ // Call the function that sends the message
+ send_picker_button_clicked_message(PICKER_BUTTON_SQUAT);
+ }
+ else
+ {
+ // Construct the setpoint
+ SetpointV2 msg_setpointV2;
+ msg_setpointV2.buttonID = PICKER_BUTTON_SQUAT;
+ msg_setpointV2.isChecked = ui->picker_squat_checkbox->isChecked();;
+ msg_setpointV2.x = 0.0;
+ msg_setpointV2.y = 0.0;
+ msg_setpointV2.z = 0.0;
+ msg_setpointV2.yaw = 0.0;
+ // Get the z coordinates from the GUI
+ if(!ui->picker_squat_z->text().isEmpty())
+ msg_setpointV2.z = (ui->picker_squat_z->text()).toFloat();
+ // Call the function that sends the message
+ send_picker_button_clicked_message_with_setpoint(msg_setpointV2);
+ }
}
-void MainWindow::on_picker_disconnect_button_clicked()
+void MainWindow::on_picker_jump_button_clicked()
{
- // Call the function that sends the message
- send_picker_button_clicked_message(PICKER_BUTTON_DISCONNECT);
+ if (!shouldSendWithSetpoint_for_pickerButtons)
+ {
+ // Call the function that sends the message
+ send_picker_button_clicked_message(PICKER_BUTTON_JUMP);
+ }
+ else
+ {
+ // Construct the setpoint
+ SetpointV2 msg_setpointV2;
+ msg_setpointV2.buttonID = PICKER_BUTTON_JUMP;
+ msg_setpointV2.isChecked = ui->picker_jump_checkbox->isChecked();;
+ msg_setpointV2.x = 0.0;
+ msg_setpointV2.y = 0.0;
+ msg_setpointV2.z = 0.0;
+ msg_setpointV2.yaw = 0.0;
+ // Get the z coordinates from the GUI
+ if(!ui->picker_jump_z->text().isEmpty())
+ msg_setpointV2.z = (ui->picker_jump_z->text()).toFloat();
+ // Call the function that sends the message
+ send_picker_button_clicked_message_with_setpoint(msg_setpointV2);
+ }
}
void MainWindow::on_picker_1_button_clicked()
{
- // Call the function that sends the message
- send_picker_button_clicked_message(PICKER_BUTTON_1);
+ if (!shouldSendWithSetpoint_for_pickerButtons)
+ {
+ // Call the function that sends the message
+ send_picker_button_clicked_message(PICKER_BUTTON_1);
+ }
+ else
+ {
+ // Construct the setpoint
+ SetpointV2 msg_setpointV2;
+ msg_setpointV2.buttonID = PICKER_BUTTON_1;
+ msg_setpointV2.isChecked = true;
+ msg_setpointV2.x = 0.0;
+ msg_setpointV2.y = 0.0;
+ msg_setpointV2.z = 0.0;
+ msg_setpointV2.yaw = 0.0;
+ // Call the function that sends the message
+ send_picker_button_clicked_message_with_setpoint(msg_setpointV2);
+ }
}
void MainWindow::on_picker_2_button_clicked()
{
- // Call the function that sends the message
- send_picker_button_clicked_message(PICKER_BUTTON_2);
+ if (!shouldSendWithSetpoint_for_pickerButtons)
+ {
+ // Call the function that sends the message
+ send_picker_button_clicked_message(PICKER_BUTTON_2);
+ }
+ else
+ {
+ // Construct the setpoint
+ SetpointV2 msg_setpointV2;
+ msg_setpointV2.buttonID = PICKER_BUTTON_2;
+ msg_setpointV2.isChecked = true;
+ msg_setpointV2.x = 0.0;
+ msg_setpointV2.y = 0.0;
+ msg_setpointV2.z = 0.0;
+ msg_setpointV2.yaw = 0.0;
+ // Call the function that sends the message
+ send_picker_button_clicked_message_with_setpoint(msg_setpointV2);
+ }
}
void MainWindow::on_picker_3_button_clicked()
{
- // Call the function that sends the message
- send_picker_button_clicked_message(PICKER_BUTTON_3);
+ if (!shouldSendWithSetpoint_for_pickerButtons)
+ {
+ // Call the function that sends the message
+ send_picker_button_clicked_message(PICKER_BUTTON_3);
+ }
+ else
+ {
+ // Construct the setpoint
+ SetpointV2 msg_setpointV2;
+ msg_setpointV2.buttonID = PICKER_BUTTON_3;
+ msg_setpointV2.isChecked = true;
+ msg_setpointV2.x = 0.0;
+ msg_setpointV2.y = 0.0;
+ msg_setpointV2.z = 0.0;
+ msg_setpointV2.yaw = 0.0;
+ // Call the function that sends the message
+ send_picker_button_clicked_message_with_setpoint(msg_setpointV2);
+ }
}
void MainWindow::on_picker_4_button_clicked()
{
- // Call the function that sends the message
- send_picker_button_clicked_message(PICKER_BUTTON_4);
+ if (!shouldSendWithSetpoint_for_pickerButtons)
+ {
+ // Call the function that sends the message
+ send_picker_button_clicked_message(PICKER_BUTTON_4);
+ }
+ else
+ {
+ // Construct the setpoint
+ SetpointV2 msg_setpointV2;
+ msg_setpointV2.buttonID = PICKER_BUTTON_4;
+ msg_setpointV2.isChecked = true;
+ msg_setpointV2.x = 0.0;
+ msg_setpointV2.y = 0.0;
+ msg_setpointV2.z = 0.0;
+ msg_setpointV2.yaw = 0.0;
+ // Call the function that sends the message
+ send_picker_button_clicked_message_with_setpoint(msg_setpointV2);
+ }
}
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 9c34555d826f627ee7e18591b5d78beed62a4cc4..60ad9e02f25b51f400e3549689202548cb40fc47 100644
--- a/pps_ws/src/d_fall_pps/include/nodes/PickerControllerService.h
+++ b/pps_ws/src/d_fall_pps/include/nodes/PickerControllerService.h
@@ -53,6 +53,7 @@
//the generated structs from the msg-files have to be included
#include "d_fall_pps/ViconData.h"
#include "d_fall_pps/Setpoint.h"
+#include "d_fall_pps/SetpointV2.h"
#include "d_fall_pps/ControlCommand.h"
#include "d_fall_pps/Controller.h"
#include "d_fall_pps/DebugMsg.h"
@@ -88,12 +89,12 @@
// FOR WHICH BUTTON WAS PRESSED IN THE PICKER CONTOLLER
#define PICKER_BUTTON_GOTOSTART 1
-#define PICKER_BUTTON_CONNECT 2
+#define PICKER_BUTTON_ATTACH 2
#define PICKER_BUTTON_PICKUP 3
#define PICKER_BUTTON_GOTOEND 4
#define PICKER_BUTTON_PUTDOWN 5
#define PICKER_BUTTON_SQUAT 6
-#define PICKER_BUTTON_DISCONNECT 7
+#define PICKER_BUTTON_JUMP 7
#define PICKER_BUTTON_1 11
#define PICKER_BUTTON_2 12
@@ -369,15 +370,17 @@ void massCallback(const std_msgs::Float32& msg);
void xAdjustmentCallback(const std_msgs::Float32& msg);
void yAdjustmentCallback(const std_msgs::Float32& msg);
+void buttonPressedWithSetpointCallback(const SetpointV2& newSetpointV2);
+
// SEPARATE CALLBACK FUNCTIONS FOR EACH BUTTON
void buttonPressed_gotoStart();
-void buttonPressed_connect();
+void buttonPressed_attach();
void buttonPressed_pickup();
void buttonPressed_gotoEnd();
void buttonPressed_putdown();
void buttonPressed_squat();
-void buttonPressed_disconnect();
+void buttonPressed_jump();
void buttonPressed_1();
void buttonPressed_2();
@@ -385,6 +388,23 @@ void buttonPressed_3();
void buttonPressed_4();
+// SEPARATE CALLBACK FUNCTIONS FOR EACH BUTTON
+// > WITH A SETPOINT IN THE MESSAGE
+void buttonPressedWithSetpoint_gotoStart(const SetpointV2& newSetpointV2);
+void buttonPressedWithSetpoint_attach(const SetpointV2& newSetpointV2);
+void buttonPressedWithSetpoint_pickup(const SetpointV2& newSetpointV2);
+void buttonPressedWithSetpoint_gotoEnd(const SetpointV2& newSetpointV2);
+void buttonPressedWithSetpoint_putdown(const SetpointV2& newSetpointV2);
+void buttonPressedWithSetpoint_squat(const SetpointV2& newSetpointV2);
+void buttonPressedWithSetpoint_jump(const SetpointV2& newSetpointV2);
+
+void buttonPressedWithSetpoint_1(const SetpointV2& newSetpointV2);
+void buttonPressedWithSetpoint_2(const SetpointV2& newSetpointV2);
+void buttonPressedWithSetpoint_3(const SetpointV2& newSetpointV2);
+void buttonPressedWithSetpoint_4(const SetpointV2& newSetpointV2);
+
+
+
diff --git a/pps_ws/src/d_fall_pps/msg/SetpointV2.msg b/pps_ws/src/d_fall_pps/msg/SetpointV2.msg
new file mode 100644
index 0000000000000000000000000000000000000000..95948a85329b5dbf5b35bb4bc22e0e68cd01f861
--- /dev/null
+++ b/pps_ws/src/d_fall_pps/msg/SetpointV2.msg
@@ -0,0 +1,6 @@
+float64 x
+float64 y
+float64 z
+float64 yaw
+bool isChecked
+uint32 buttonID
\ No newline at end of file
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 4e17a0784c30f28af7cb8059e5fad3fcdb442355..d57d09766d6c284578d65fe27152d03ddbbe1376 100644
--- a/pps_ws/src/d_fall_pps/src/nodes/PickerControllerService.cpp
+++ b/pps_ws/src/d_fall_pps/src/nodes/PickerControllerService.cpp
@@ -207,9 +207,9 @@ void buttonPressed_gotoStart()
publishCurrentSetpoint();
}
-void buttonPressed_connect()
+void buttonPressed_attach()
{
- ROS_INFO("[PICKER CONTROLLER] Connect button pressed");
+ ROS_INFO("[PICKER CONTROLLER] Attach button pressed");
m_shouldSmoothSetpointChanges = true;
m_setpoint[2] = m_picker_string_length + m_thickness_of_object_at_pickup;
@@ -256,9 +256,9 @@ void buttonPressed_squat()
publishCurrentSetpoint();
}
-void buttonPressed_disconnect()
+void buttonPressed_jump()
{
- ROS_INFO("[PICKER CONTROLLER] Disconnect button pressed");
+ ROS_INFO("[PICKER CONTROLLER] Jump button pressed");
m_shouldSmoothSetpointChanges = false;
m_setpoint[2] = m_picker_string_length + 0.10;
@@ -363,6 +363,140 @@ void yAdjustmentCallback(const std_msgs::Float32& msg)
+// CALLBACK FUNCTION THAT RUN WHEN THE RESPECTIVE BUTTON IS PRESSED
+// > AND A SETPOINT IS PROVIDED
+
+void buttonPressedWithSetpoint_gotoStart(const SetpointV2& newSetpointV2)
+{
+ ROS_INFO("[PICKER CONTROLLER] Goto Start button pressed with a setpoint provided");
+
+ // Use the boolean for the smoothing flag
+ m_shouldSmoothSetpointChanges = newSetpointV2.isChecked;
+ // Set the (x,y,z) coordinates:
+ m_setpoint[0] = newSetpointV2.x;
+ m_setpoint[1] = newSetpointV2.y;
+ m_setpoint[2] = newSetpointV2.z;
+ // Publish the setpoint so that the GUI updates
+ publishCurrentSetpoint();
+}
+
+void buttonPressedWithSetpoint_attach(const SetpointV2& newSetpointV2)
+{
+ ROS_INFO("[PICKER CONTROLLER] Attach button pressed with a setpoint provided");
+
+ // Use the boolean for the smoothing flag
+ m_shouldSmoothSetpointChanges = newSetpointV2.isChecked;
+ // Set the z coordinate:
+ m_setpoint[2] = newSetpointV2.z;
+ // Publish the setpoint so that the GUI updates
+ publishCurrentSetpoint();
+}
+
+void buttonPressedWithSetpoint_pickup(const SetpointV2& newSetpointV2)
+{
+ ROS_INFO("[PICKER CONTROLLER] Pick up button pressed with a setpoint provided");
+
+ // Use the boolean for the smoothing flag
+ m_shouldSmoothSetpointChanges = newSetpointV2.isChecked;
+ // Set the z coordinate:
+ m_setpoint[2] = newSetpointV2.z;
+ // Update the mass of the Crazyflie
+ m_mass_total_grams = m_mass_CF_grams + m_mass_E_grams;
+ // Publish the setpoint so that the GUI updates
+ publishCurrentSetpoint();
+}
+
+void buttonPressedWithSetpoint_gotoEnd(const SetpointV2& newSetpointV2)
+{
+ ROS_INFO("[PICKER CONTROLLER] Goto End button pressed with a setpoint provided");
+
+ // Use the boolean for the smoothing flag
+ m_shouldSmoothSetpointChanges = newSetpointV2.isChecked;
+ // Set the (x,y) coordinates:
+ m_setpoint[0] = newSetpointV2.x;
+ m_setpoint[1] = newSetpointV2.y;
+ // Publish the setpoint so that the GUI updates
+ publishCurrentSetpoint();
+}
+
+void buttonPressedWithSetpoint_putdown(const SetpointV2& newSetpointV2)
+{
+ ROS_INFO("[PICKER CONTROLLER] Put down button pressed with a setpoint provided");
+
+ // Use the boolean for the smoothing flag
+ m_shouldSmoothSetpointChanges = newSetpointV2.isChecked;
+ // Set the z coordinate:
+ m_setpoint[2] = newSetpointV2.z;
+ // Update the mass of the Crazyflie
+ m_mass_total_grams = m_mass_CF_grams;
+ // Publish the setpoint so that the GUI updates
+ publishCurrentSetpoint();
+}
+
+void buttonPressedWithSetpoint_squat(const SetpointV2& newSetpointV2)
+{
+ ROS_INFO("[PICKER CONTROLLER] Squat button pressed with a setpoint provided");
+
+ // Use the boolean for the smoothing flag
+ m_shouldSmoothSetpointChanges = newSetpointV2.isChecked;
+ // Set the z coordinate:
+ m_setpoint[2] = newSetpointV2.z;
+ // Update the mass of the Crazyflie
+ m_mass_total_grams = m_mass_CF_grams;
+ // Publish the setpoint so that the GUI updates
+ publishCurrentSetpoint();
+}
+
+void buttonPressedWithSetpoint_jump(const SetpointV2& newSetpointV2)
+{
+ ROS_INFO("[PICKER CONTROLLER] Jump button pressed with a setpoint provided");
+
+ // Use the boolean for the smoothing flag
+ m_shouldSmoothSetpointChanges = newSetpointV2.isChecked;
+ // Set the z coordinate:
+ m_setpoint[2] = newSetpointV2.z;
+ // Update the mass of the Crazyflie
+ m_mass_total_grams = m_mass_CF_grams;
+ // Publish the setpoint so that the GUI updates
+ publishCurrentSetpoint();
+}
+
+
+void buttonPressedWithSetpoint_1(const SetpointV2& newSetpointV2)
+{
+ ROS_INFO("[PICKER CONTROLLER] Button 1 pressed with a setpoint provided");
+}
+
+void buttonPressedWithSetpoint_2(const SetpointV2& newSetpointV2)
+{
+ ROS_INFO("[PICKER CONTROLLER] Button 2 pressed with a setpoint provided");
+}
+
+void buttonPressedWithSetpoint_3(const SetpointV2& newSetpointV2)
+{
+ ROS_INFO("[PICKER CONTROLLER] Button 3 pressed with a setpoint provided");
+}
+
+void buttonPressedWithSetpoint_4(const SetpointV2& newSetpointV2)
+{
+ ROS_INFO("[PICKER CONTROLLER] Button 4 pressed with a setpoint provided with a setpoint provided");
+}
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
// ------------------------------------------------------------------------------
// OOO U U TTTTT EEEEE RRRR
@@ -887,13 +1021,13 @@ void convert_stateInertial_to_bodyFrameError(float stateInertial[12], float setp
stateInertial[1] = stateInertial[1] - setpoint[1] - m_yAdjustment;
stateInertial[2] = stateInertial[2] - setpoint[2];
- if (stateInertial[2] > 30.0f)
+ if (stateInertial[2] > 40.0f)
{
- stateInertial[2] = 30.0f;
+ stateInertial[2] = 40.0f;
}
- else if (stateInertial[2] < -30.0f)
+ else if (stateInertial[2] < -40.0f)
{
- stateInertial[2] = 30.0f;
+ stateInertial[2] = -40.0f;
}
// Fill in the yaw angle error
@@ -1027,8 +1161,8 @@ void buttonPressedCallback(const std_msgs::Int32& msg)
case PICKER_BUTTON_GOTOSTART:
buttonPressed_gotoStart();
break;
- case PICKER_BUTTON_CONNECT:
- buttonPressed_connect();
+ case PICKER_BUTTON_ATTACH:
+ buttonPressed_attach();
break;
case PICKER_BUTTON_PICKUP:
buttonPressed_pickup();
@@ -1042,8 +1176,8 @@ void buttonPressedCallback(const std_msgs::Int32& msg)
case PICKER_BUTTON_SQUAT:
buttonPressed_squat();
break;
- case PICKER_BUTTON_DISCONNECT:
- buttonPressed_disconnect();
+ case PICKER_BUTTON_JUMP:
+ buttonPressed_jump();
break;
case PICKER_BUTTON_1:
buttonPressed_1();
@@ -1068,6 +1202,58 @@ void buttonPressedCallback(const std_msgs::Int32& msg)
+void buttonPressedWithSetpointCallback(const SetpointV2& newSetpointV2)
+{
+ // Extract the "buttonID" from the message
+ int button_index = newSetpointV2.buttonID;
+
+ // Switch between the button pressed
+ switch(button_index)
+ {
+ case PICKER_BUTTON_GOTOSTART:
+ buttonPressedWithSetpoint_gotoStart(newSetpointV2);
+ break;
+ case PICKER_BUTTON_ATTACH:
+ buttonPressedWithSetpoint_attach(newSetpointV2);
+ break;
+ case PICKER_BUTTON_PICKUP:
+ buttonPressedWithSetpoint_pickup(newSetpointV2);
+ break;
+ case PICKER_BUTTON_GOTOEND:
+ buttonPressedWithSetpoint_gotoEnd(newSetpointV2);
+ break;
+ case PICKER_BUTTON_PUTDOWN:
+ buttonPressedWithSetpoint_putdown(newSetpointV2);
+ break;
+ case PICKER_BUTTON_SQUAT:
+ buttonPressedWithSetpoint_squat(newSetpointV2);
+ break;
+ case PICKER_BUTTON_JUMP:
+ buttonPressedWithSetpoint_jump(newSetpointV2);
+ break;
+ case PICKER_BUTTON_1:
+ buttonPressedWithSetpoint_1(newSetpointV2);
+ break;
+ case PICKER_BUTTON_2:
+ buttonPressedWithSetpoint_2(newSetpointV2);
+ break;
+ case PICKER_BUTTON_3:
+ buttonPressedWithSetpoint_3(newSetpointV2);
+ break;
+ case PICKER_BUTTON_4:
+ buttonPressedWithSetpoint_4(newSetpointV2);
+ break;
+ default:
+ {
+ // Let the user know that the command was not recognised
+ ROS_INFO_STREAM("[PICKER CONTROLLER] A button pressed message was received in the controller but not recognised, message.data = " << button_index );
+ break;
+ }
+ }
+}
+
+
+
// ************************************************************************************************
// PPPP U U BBBB L III SSSS H H X X Y Y ZZZZZ Y Y A W W
// P P U U B B L I S H H X X Y Y Z Y Y A A W W
@@ -1502,6 +1688,8 @@ int main(int argc, char* argv[]) {
pickerSetpointToGUIPublisher = nodeHandle.advertise<Setpoint>("SetpointToGUI", 1);
+ ros::Subscriber pickerButtonPressedWithSetpointSubscriber = nodeHandle.subscribe("ButtonPressedWithSetpoint", 1, buttonPressedWithSetpointCallback);
+
// Print out some information to the user.