To receive notifications about scheduled maintenance, please subscribe to the mailing-list gitlab-operations@sympa.ethz.ch. You can subscribe to the mailing-list at https://sympa.ethz.ch

Commit 9a2930e9 authored by roangel's avatar roangel
Browse files

Small adjustments and tested to successfully pick-up and vaguely drop-off a letter

parent c788a4f3
......@@ -331,6 +331,7 @@ private:
ros::Publisher pickerYAdjustmentPublisher;
ros::Publisher pickerSetpointPublisher;
ros::Subscriber pickerSetpointSubscriber;
ros::Subscriber pickerSetpointToGUISubscriber;
......
......@@ -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 );
......
......@@ -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);
......
......@@ -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>
......
# 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
......
......@@ -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.
......
Markdown is supported
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