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 56c3fefb7c2c12c4e0c78bd2060c4f04c12c0729..005cb43082f8cc6629abc491dc04f1fbcccc8859 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
@@ -331,6 +331,7 @@ private:
ros::Publisher pickerYAdjustmentPublisher;
ros::Publisher pickerSetpointPublisher;
ros::Subscriber pickerSetpointSubscriber;
+ ros::Subscriber pickerSetpointToGUISubscriber;
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 59d5a0aa562f25d6000ff2f0c6e975285205386c..ba915532a997881494a87abed4325d51ca287738 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
@@ -107,6 +107,7 @@ MainWindow::MainWindow(int argc, char **argv, QWidget *parent) :
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);
+ pickerSetpointToGUISubscriber = nodeHandle.subscribe("PickerControllerService/SetpointToGUI", 1, &MainWindow::pickerSetpointCallback, this);
// SET ALL SLIDERS AND DIALS TO DEFAULT VALUES
ui->picker_z_slider->setValue( 40 );
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 676bae219f7e2df7d168608a0ddaaa854bfd7a2d..04cc2b2d890a58871fd500a0172774db4ff052b3 100644
--- a/pps_ws/src/d_fall_pps/include/nodes/PickerControllerService.h
+++ b/pps_ws/src/d_fall_pps/include/nodes/PickerControllerService.h
@@ -179,6 +179,10 @@ 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);
+// Length of the string from the Crazyflie
+// to the end of the Picker, in [meters]
+float m_picker_string_length;
+
// > 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};
@@ -271,6 +275,10 @@ ros::Publisher debugPublisher;
// Boolean whether to execute the convert into body frame function
bool shouldPerformConvertIntoBodyFrame = false;
+// Boolean for whether to clip the current position to setpoint distance
+bool shouldSmoothVerticalSetpointChanges = false;
+bool shouldSmoothHorizonatalSetpointChanges = false;
+
// VARIABLES RELATING TO THE PUBLISHING OF A DEBUG MESSAGE
@@ -296,6 +304,9 @@ bool shouldPublishCurrent_xyz_yaw = false;
// ROS Publisher for my current (x,y,z,yaw) position
ros::Publisher my_current_xyz_yaw_publisher;
+// ROS Publisher for the current setpoint
+ros::Publisher pickerSetpointToGUIPublisher;
+
// RELEVANT NOTES ABOUT THE VARIABLES DECLARE HERE:
// The "CrazyflieData" type used for the "request" variable is a
@@ -399,6 +410,8 @@ float computeMotorPolyBackward(float thrust);
void setpointCallback(const Setpoint& newSetpoint);
void xyz_yaw_to_follow_callback(const Setpoint& newSetpoint);
+void publishCurrentSetpoint();
+
// CUSTOM COMMAND RECEIVED CALLBACK
//void customCommandReceivedCallback(const CustomButton& commandReceived);
diff --git a/pps_ws/src/d_fall_pps/launch/Agent.launch b/pps_ws/src/d_fall_pps/launch/Agent.launch
index eb8c339ac53843e8855edf4366545255468fb320..a24b9ad1c03a1e998038f6978487b545dae47259 100755
--- a/pps_ws/src/d_fall_pps/launch/Agent.launch
+++ b/pps_ws/src/d_fall_pps/launch/Agent.launch
@@ -142,6 +142,11 @@
file = "$(find d_fall_pps)/param/TuningController.yaml"
ns = "TuningController"
/>
+ <rosparam
+ command = "load"
+ file = "$(find d_fall_pps)/param/PickerController.yaml"
+ ns = "PickerController"
+ />
</node>
diff --git a/pps_ws/src/d_fall_pps/param/PickerController.yaml b/pps_ws/src/d_fall_pps/param/PickerController.yaml
index 7723335dfcd33c5c1a52c8c01b54fb32cc4f2722..e20851cd0fe96b6acace8f0eb510f4909cf009e2 100644
--- a/pps_ws/src/d_fall_pps/param/PickerController.yaml
+++ b/pps_ws/src/d_fall_pps/param/PickerController.yaml
@@ -1,18 +1,21 @@
# Mass of the crazyflie
-mass_CF : 29
+mass_CF : 32
# Mass of the letters
-mass_E : 4
-mass_T : 3
-mass_H : 4
+mass_E : 3.2
+mass_T : 2.9
+mass_H : 3.3
# (x,y) coordinates of the pickup location
pickup_coordinates_xy : [-0.3, 0.0]
# (x,y) coordinates of the drop off location
-dropoff_coordinates_xy_for_E : [0.30, 0.0]
-dropoff_coordinates_xy_for_T : [0.34, 0.0]
-dropoff_coordinates_xy_for_H : [0.38, 0.0]
+dropoff_coordinates_xy_for_E : [0.20, 0.0]
+dropoff_coordinates_xy_for_T : [0.24, 0.0]
+dropoff_coordinates_xy_for_H : [0.28, 0.0]
+# Length of the string from the Crazyflie
+# to the end of the Picker, in [meters]
+picker_string_length : 0.38
@@ -30,6 +33,10 @@ motorPoly : [5.484560e-4, 1.032633e-6, 2.130295e-11]
# Boolean for whether to execute the convert into body frame function
shouldPerformConvertIntoBodyFrame : true
+# Boolean for whether to clip the current position to setpoint distance
+shouldSmoothVerticalSetpointChanges : true
+shouldSmoothHorizontalSetpointChanges : true
+
# Boolean indiciating whether the (x,y,z,yaw) of this agent should be published or not
shouldPublishCurrent_xyz_yaw : true
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 ec9cbf4d8971df66d6222fde6d78fc85168836e1..de0de83d528f2ab3583978bda4973e09679b2beb 100644
--- a/pps_ws/src/d_fall_pps/src/nodes/PickerControllerService.cpp
+++ b/pps_ws/src/d_fall_pps/src/nodes/PickerControllerService.cpp
@@ -87,30 +87,57 @@
void buttonPressed_gotoStart()
{
ROS_INFO("[PICKER CONTROLLER] Goto Start button pressed");
+
+ shouldSmoothVerticalSetpointChanges = true;
+ m_setpoint[0] = m_pickup_coordinates_xy[0];
+ m_setpoint[1] = m_pickup_coordinates_xy[1];
+ m_setpoint[2] = m_picker_string_length + 0.10;
+ publishCurrentSetpoint();
}
void buttonPressed_connect()
{
ROS_INFO("[PICKER CONTROLLER] Connect button pressed");
+
+ m_setpoint[2] = m_picker_string_length + 0.01;
+ publishCurrentSetpoint();
}
void buttonPressed_pickup()
{
ROS_INFO("[PICKER CONTROLLER] Pick up button pressed");
+
+ m_mass_total_grams = m_mass_CF_grams + m_mass_E_grams;
+ m_setpoint[2] = m_picker_string_length + 0.15;
+ publishCurrentSetpoint();
}
+
void buttonPressed_gotoEnd()
{
ROS_INFO("[PICKER CONTROLLER] Goto End button pressed");
+
+ m_setpoint[0] = m_dropoff_coordinates_xy_for_E[0];
+ m_setpoint[1] = m_dropoff_coordinates_xy_for_E[1];
+ publishCurrentSetpoint();
}
void buttonPressed_putdown()
{
ROS_INFO("[PICKER CONTROLLER] Put down button pressed");
+
+ m_setpoint[2] = m_picker_string_length - 0.10;
+ m_mass_total_grams = m_mass_CF_grams;
+ publishCurrentSetpoint();
}
void buttonPressed_disconnect()
{
ROS_INFO("[PICKER CONTROLLER] Disconnect button pressed");
+
+ shouldSmoothVerticalSetpointChanges = false;
+ m_setpoint[2] = m_picker_string_length + 0.15;
+ m_mass_total_grams = m_mass_CF_grams;
+ publishCurrentSetpoint();
}
void buttonPressed_1()
@@ -725,6 +752,18 @@ 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 (shouldSmoothVerticalSetpointChanges)
+ {
+ if (stateInertial[2] > 10.0f)
+ {
+ stateInertial[2] = 10.0f;
+ }
+ else if (stateInertial[2] < -10.0f)
+ {
+ stateInertial[2] = 10.0f;
+ }
+ }
+
// Fill in the yaw angle error
// > This error should be "unwrapped" to be in the range
// ( -pi , pi )
@@ -818,6 +857,17 @@ void setpointCallback(const Setpoint& newSetpoint)
}
+void publishCurrentSetpoint()
+{
+ Setpoint msg_setpoint;
+ msg_setpoint.x = m_setpoint[0];
+ msg_setpoint.y = m_setpoint[1];
+ msg_setpoint.z = m_setpoint[2];
+ msg_setpoint.yaw = m_setpoint[3];
+
+ pickerSetpointToGUIPublisher.publish(msg_setpoint);
+}
+
// ----------------------------------------------------------------------------------
// CCCC U U SSSS TTTTT OOO M M
@@ -995,6 +1045,9 @@ void fetchYamlParameters(ros::NodeHandle& nodeHandle)
getParameterFloatVector(nodeHandle_for_pickerController, "dropoff_coordinates_xy_for_T", m_dropoff_coordinates_xy_for_T, 2);
getParameterFloatVector(nodeHandle_for_pickerController, "dropoff_coordinates_xy_for_H", m_dropoff_coordinates_xy_for_H, 2);
+ // Length of the string from the Crazyflie to the end of the Picker, in [meters]
+ m_picker_string_length = getParameterFloat(nodeHandle_for_pickerController , "picker_string_length");
+
// THE FOLLOWING PARAMETERS ARE USED FOR THE LOW-LEVEL CONTROLLER
@@ -1013,6 +1066,10 @@ void fetchYamlParameters(ros::NodeHandle& nodeHandle)
// > The boolean for whether to execute the convert into body frame function
shouldPerformConvertIntoBodyFrame = getParameterBool(nodeHandle_for_pickerController, "shouldPerformConvertIntoBodyFrame");
+ // Boolean for whether to clip the current position to setpoint distance
+ shouldSmoothVerticalSetpointChanges = getParameterBool(nodeHandle_for_pickerController, "shouldSmoothVerticalSetpointChanges");
+ shouldSmoothHorizonatalSetpointChanges = getParameterBool(nodeHandle_for_pickerController, "shouldSmoothHorizonatalSetpointChanges");
+
// > The boolean indicating whether the (x,y,z,yaw) of this agent should be published
// or not
shouldPublishCurrent_xyz_yaw = getParameterBool(nodeHandle_for_pickerController, "shouldPublishCurrent_xyz_yaw");
@@ -1300,6 +1357,8 @@ int main(int argc, char* argv[]) {
ros::Subscriber pickerXAdjustmentSubscriber = nodeHandle.subscribe("XAdjustment", 1, xAdjustmentCallback);
ros::Subscriber pickerYAdjustmentSubscriber = nodeHandle.subscribe("YAdjustment", 1, yAdjustmentCallback);
+ pickerSetpointToGUIPublisher = nodeHandle.advertise<Setpoint>("SetpointToGUI", 1);
+
// Print out some information to the user.