Commit 6aad68b0 authored by beuchatp's avatar beuchatp
Browse files

Removed all the commented out old code from the Picker Controller

parent 530c1f72
......@@ -65,12 +65,6 @@
#include "dfall_pkg/Controller.h"
#include "dfall_pkg/DebugMsg.h"
//the generated structs from the msg-files have to be included
// #include "dfall_pkg/Setpoint.h"
// #include "dfall_pkg/SetpointV2.h"
// #include "dfall_pkg/ControlCommand.h"
// #include "dfall_pkg/CustomButton.h"
// Include the DFALL service types
#include "dfall_pkg/LoadYamlFromFilename.h"
#include "dfall_pkg/GetSetpointService.h"
......@@ -106,39 +100,6 @@ using namespace dfall_pkg;
// These constants are defined to make the code more readable and adaptable.
// FOR WHICH BUTTON WAS PRESSED IN THE PICKER CONTOLLER
// #define PICKER_BUTTON_GOTOSTART 1
// #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_JUMP 7
// #define PICKER_BUTTON_1 11
// #define PICKER_BUTTON_2 12
// #define PICKER_BUTTON_3 13
// #define PICKER_BUTTON_4 14
// These constants define the modes that can be used for controller the Crazyflie 2.0,
// the constants defined here need to be in agreement with those defined in the
// firmware running on the Crazyflie 2.0.
// The following is a short description about each mode:
// MOTOR_MODE In this mode the Crazyflie will apply the requested 16-bit per motor
// command directly to each of the motors
// RATE_MODE In this mode the Crazyflie will apply the requested 16-bit per motor
// command directly to each of the motors, and additionally request the
// body frame roll, pitch, and yaw angular rates from the PID rate
// controllers implemented in the Crazyflie 2.0 firmware.
// ANGE_MODE In this mode the Crazyflie will apply the requested 16-bit per motor
// command directly to each of the motors, and additionally request the
// body frame roll, pitch, and yaw angles from the PID attitude
// controllers implemented in the Crazyflie 2.0 firmware.
// #define CF_COMMAND_TYPE_MOTOR 6
// #define CF_COMMAND_TYPE_RATE 7
// #define CF_COMMAND_TYPE_ANGLE 8
// These constants define the method used for estimating the Inertial
// frame state.
// All methods are run at all times, this flag indicates which estimate
......@@ -331,184 +292,6 @@ ros::Publisher m_setpointChangedPublisher;
// // Current time
// int m_time_ticks = 0;
// float m_time_seconds;
// // > Mass of the Crazyflie quad-rotor, in [grams]
// float m_mass_CF_grams;
// // > Mass of the letters to be lifted, in [grams]
// float m_mass_E_grams;
// float m_mass_T_grams;
// float m_mass_H_grams;
// // > Total mass of the Crazyflie plus whatever it is carrying, in [grams]
// float m_mass_total_grams;
// // Thickness of the object at pick-up and put-down, in [meters]
// // > This should also account for extra height due to
// // the surface where the object is
// float m_thickness_of_object_at_pickup;
// float m_thickness_of_object_at_putdown;
// // (x,y) coordinates of the pickup location
// std::vector<float> m_pickup_coordinates_xy(2);
// // (x,y) coordinates of the drop off location
// 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};
// float m_setpoint_for_controller[4] = {0.0,0.0,0.4,0.0};
// // > Small adjustments to the x-y setpoint
// float m_xAdjustment = 0.0f;
// float m_yAdjustment = 0.0f;
// // Boolean for whether to limit rate of change of the setpoint
// bool m_shouldSmoothSetpointChanges = true;
// // Max setpoint change per second
// float m_max_setpoint_change_per_second_horizontal;
// float m_max_setpoint_change_per_second_vertical;
// float m_max_setpoint_change_per_second_yaw_degrees;
// float m_max_setpoint_change_per_second_yaw_radians;
// // Frequency at which the controller is running
// float m_vicon_frequency;
// THE FOLLOWING PARAMETERS ARE USED
// FOR THE LOW-LEVEL CONTROLLER
// // Frequency at which the controller is running
// float control_frequency;
// // > Coefficients of the 16-bit command to thrust conversion
// std::vector<float> motorPoly(3);
// // The LQR Controller parameters for "CONTROLLER_MODE_LQR_RATE"
// std::vector<float> gainMatrixThrust_NineStateVector (9,0.0);
// std::vector<float> gainMatrixRollRate (9,0.0);
// std::vector<float> gainMatrixPitchRate (9,0.0);
// std::vector<float> gainMatrixYawRate (9,0.0);
// // The 16-bit command limits
// float cmd_sixteenbit_min;
// float cmd_sixteenbit_max;
// // VARIABLES FOR THE ESTIMATOR
// // Frequency at which the controller is running
// float estimator_frequency;
// // > A flag for which estimator to use:
// int estimator_method = ESTIMATOR_METHOD_FINITE_DIFFERENCE;
// // > The current state interial estimate,
// // for use by the controller
// float current_stateInertialEstimate[12];
// // > The measurement of the Crazyflie at the "current" time step,
// // to avoid confusion
// float current_xzy_rpy_measurement[6];
// // > The measurement of the Crazyflie at the "previous" time step,
// // used for computing finite difference velocities
// float previous_xzy_rpy_measurement[6];
// // > The full 12 state estimate maintained by the finite
// // difference state estimator
// float stateInterialEstimate_viaFiniteDifference[12];
// // > The full 12 state estimate maintained by the point mass
// // kalman filter state estimator
// float stateInterialEstimate_viaPointMassKalmanFilter[12];
// // THE POINT MASS KALMAN FILTER (PMKF) GAINS AND ERROR EVOLUATION
// // > For the (x,y,z) position
// std::vector<float> PMKF_Ahat_row1_for_positions (2,0.0);
// std::vector<float> PMKF_Ahat_row2_for_positions (2,0.0);
// std::vector<float> PMKF_Kinf_for_positions (2,0.0);
// // > For the (roll,pitch,yaw) angles
// std::vector<float> PMKF_Ahat_row1_for_angles (2,0.0);
// std::vector<float> PMKF_Ahat_row2_for_angles (2,0.0);
// std::vector<float> PMKF_Kinf_for_angles (2,0.0);
// // VARIABLES FOR THE NAMESPACES FOR THE PARAMETER SERVICES
// // > For the paramter service of this agent
// std::string namespace_to_own_agent_parameter_service;
// // > For the parameter service of the coordinator
// std::string namespace_to_coordinator_parameter_service;
// // ROS PUBLISHER FOR SENDING OUT THE DEBUG MESSAGES
// ros::Publisher debugPublisher;
// // VARIABLES RELATING TO PERFORMING THE CONVERSION INTO BODY FRAME
// // Boolean whether to execute the convert into body frame function
// bool shouldPerformConvertIntoBodyFrame = false;
// // VARIABLES RELATING TO THE PUBLISHING OF A DEBUG MESSAGE
// // Boolean indiciating whether the "Debug Message" of this agent should be published or not
// bool shouldPublishDebugMessage = false;
// // Boolean indiciating whether the debugging ROS_INFO_STREAM should be displayed or not
// bool shouldDisplayDebugInfo = false;
// // VARIABLES RELATING TO PUBLISHING CURRENT POSITION AND FOLLOWING ANOTHER AGENT'S
// // POSITION
// // The ID of this agent, i.e., the ID of this compute
// int my_agentID = 0;
// // Boolean indicating whether the (x,y,z,yaw) of this agent should be published or not
// // > The default behaviour is: do not publish,
// // > This varaible is changed based on parameters loaded from the YAML file
// 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
// structure as defined in the file "CrazyflieData.msg" which has the following
// properties:
// string crazyflieName The name given to the Crazyflie in the Vicon software
// float64 x The x position of the Crazyflie [metres]
// float64 y The y position of the Crazyflie [metres]
// float64 z The z position of the Crazyflie [metres]
// float64 roll The roll component of the intrinsic Euler angles [radians]
// float64 pitch The pitch component of the intrinsic Euler angles [radians]
// float64 yaw The yaw component of the intrinsic Euler angles [radians]
// float64 acquiringTime #delta t The time elapsed since the previous "CrazyflieData" was received [seconds]
// bool occluded A boolean indicted whether the Crazyflie for visible at the time of this measurement
// ----------------------------------------------------------------------------------
......@@ -535,16 +318,6 @@ ros::Publisher m_setpointChangedPublisher;
// ADDED FOR THE PICKER
void perControlCycleOperations();
// CALLBACK FROM ROS MESSAGES RECEIVED
//void buttonPressedCallback(const std_msgs::Int32& msg);
// void zSetpointCallback(const std_msgs::Float32& msg);
// void yawSetpointCallback(const std_msgs::Float32& msg);
// 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_goto_start();
......@@ -556,30 +329,6 @@ void buttonPressed_squat();
void buttonPressed_jump();
void buttonPressed_standby();
// void buttonPressed_1();
// void buttonPressed_2();
// 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);
......@@ -629,17 +378,5 @@ bool getCurrentSetpointCallback(GetSetpointService::Request &request, GetSetpoin
void publish_current_xyz_yaw(float x, float y, float z, float yaw);
// FOR LOADING THE YAML PARAMETERS
// float getParameterFloat(ros::NodeHandle& nodeHandle, std::string name);
// void getParameterFloatVector(ros::NodeHandle& nodeHandle, std::string name, std::vector<float>& val, int length);
// int getParameterInt(ros::NodeHandle& nodeHandle, std::string name);
// void getParameterIntVectorWithKnownLength(ros::NodeHandle& nodeHandle, std::string name, std::vector<int>& val, int length);
// int getParameterIntVectorWithUnknownLength(ros::NodeHandle& nodeHandle, std::string name, std::vector<int>& val);
// bool getParameterBool(ros::NodeHandle& nodeHandle, std::string name);
void isReadyPickerControllerYamlCallback(const IntWithHeader & msg);
void fetchPickerControllerYamlParameters(ros::NodeHandle& nodeHandle);
//void yamlReadyForFetchCallback(const std_msgs::Int32& msg);
//void fetchYamlParameters(ros::NodeHandle& nodeHandle);
//void processFetchedParameters();
void fetchPickerControllerYamlParameters(ros::NodeHandle& nodeHandle);
\ No newline at end of file
......@@ -6,33 +6,6 @@ max_setpoint_change_per_second_yaw_degrees : 90.00 # [degrees]
# Mass of the letters
#mass_E : 3.2
#mass_T : 2.9
#mass_H : 3.3
# Thickness of the object at pick-up and put-down, in [meters]
# > This should also account for extra height due to
# the surface where the object is
#thickness_of_object_at_pickup : 0.02
#thickness_of_object_at_putdown : 0.03
# (x,y) coordinates of the pickup location
#pickup_coordinates_xy : [-0.26, 0.12]
# (x,y) coordinates of the drop off location
#dropoff_coordinates_xy_for_E : [ 0.33, -0.03]
#dropoff_coordinates_xy_for_T : [ 0.24, 0.00]
#dropoff_coordinates_xy_for_H : [ 0.28, 0.00]
# Length of the string from the Crazyflie
# to the end of the Picker, in [meters]
#picker_string_length : 0.30
......
Supports Markdown
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