Adjust the Picker Controller Node to be integrated with the GUI. Needs compiling and testing.

pps_ws/src/d_fall_pps/include/nodes/PickerControllerConstants.h

@@ -36,7 +36,7 @@
 // TO IDENITFY THE STATE OF THE PICKER
 
 #define PICKER_STATE_UNKNOWN      -1
-#define PICKER_STATE_STANDBY       0
+#define PICKER_STATE_STANDBY      99
 #define PICKER_STATE_GOTO_START    1
 #define PICKER_STATE_ATTACH        2
 #define PICKER_STATE_LIFT_UP       3

pps_ws/src/d_fall_pps/include/nodes/PickerControllerService.h

@@ -178,12 +178,18 @@ using namespace d_fall_pps;
 // ----------------------------------------
 // VARIABLES SPECIFIC TO THE PICKER SERVICE
 
+// The current state of the picker, i.e.,
+// {goto start, attach, lift up, goto end, put down,
+//  squat, jump, standby}
+int m_picker_current_state = PICKER_STATE_STANDBY;
+
 // Current time
-int m_time_ticks = 0;
-float m_time_seconds;
+//int m_time_ticks = 0;
+//float m_time_seconds;
 
 // > Total mass of the Crazyflie plus whatever it is carrying, in [grams]
-float m_mass_total_grams;
+float m_mass_total_in_grams;
+float m_weight_total_in_newtons;
 
 // The setpoint to be tracked, the ordering is (x,y,z,yaw),
 // with units [meters,meters,meters,radians]
@@ -226,7 +232,7 @@ std::string m_namespace_to_coordinator_parameter_service;
 // VARIABLES FOR THE CONTOLLER
 
 // > the mass of the crazyflie, in [grams]
-float yaml_cf_mass_in_grams = 25.0;
+float yaml_mass_cf_in_grams = 25.0;
 
 // > the coefficients of the 16-bit command to thrust conversion
 //std::vector<float> yaml_motorPoly(3);
@@ -240,10 +246,10 @@ float yaml_control_frequency = 200.0;
 std::vector<float> yaml_default_setpoint = {0.0,0.0,0.4,0.0};
 
 // The weight of the Crazyflie in Newtons, i.e., mg
-float m_cf_weight_in_newtons = 0.0;
+float m_weight_cf_in_newtons = 0.0;
 
 // The location error of the Crazyflie at the "previous" time step
-float m_previous_stateErrorInertial[9];
+std::vector<float> m_previous_stateErrorInertial = {0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0};
 
 // The LQR Controller parameters for "CONTROLLER_MODE_LQR_RATE"
 std::vector<float> yaml_gainMatrixThrust_NineStateVector (9,0.0);
@@ -252,15 +258,15 @@ std::vector<float> yaml_gainMatrixPitchRate              (9,0.0);
 std::vector<float> yaml_gainMatrixYawRate                (9,0.0);
 
 // The 16-bit command limits
-float yaml_cmd_sixteenbit_min;
-float yaml_cmd_sixteenbit_max;
+float yaml_cmd_sixteenbit_min = 1000;
+float yaml_cmd_sixteenbit_max = 60000;
 
 
 
 // VARIABLES FOR THE ESTIMATOR
 
 // Frequency at which the controller is running
-float yaml_estimator_frequency;
+float yaml_estimator_frequency = 200.0;
 
 // > A flag for which estimator to use:
 int yaml_estimator_method = ESTIMATOR_METHOD_FINITE_DIFFERENCE;
@@ -299,16 +305,16 @@ std::vector<float> yaml_PMKF_Kinf_for_angles         (2,0.0);
 // VARIABLES RELATING TO PERFORMING THE CONVERSION INTO BODY FRAME
 
 // Boolean whether to execute the convert into body frame function
-bool shouldPerformConvertIntoBodyFrame = false;
+bool yaml_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;
+bool yaml_shouldPublishDebugMessage = false;
 
 // Boolean indiciating whether the debugging ROS_INFO_STREAM should be displayed or not
-bool shouldDisplayDebugInfo = false;
+bool yaml_shouldDisplayDebugInfo = false;
 
 // ROS Publisher for debugging variables
 ros::Publisher m_debugPublisher;
@@ -530,45 +536,46 @@ ros::Publisher m_setpointChangedPublisher;
 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 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_gotoStart();
+void buttonPressed_goto_start();
 void buttonPressed_attach();
-void buttonPressed_pickup();
-void buttonPressed_gotoEnd();
-void buttonPressed_putdown();
+void buttonPressed_lift_up();
+void buttonPressed_goto_end();
+void buttonPressed_put_down();
 void buttonPressed_squat();
 void buttonPressed_jump();
+void buttonPressed_standby();
 
-void buttonPressed_1();
-void buttonPressed_2();
-void buttonPressed_3();
-void buttonPressed_4();
+// 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_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);
+// void buttonPressedWithSetpoint_1(const SetpointV2& newSetpointV2);
+// void buttonPressedWithSetpoint_2(const SetpointV2& newSetpointV2);
+// void buttonPressedWithSetpoint_3(const SetpointV2& newSetpointV2);
+// void buttonPressedWithSetpoint_4(const SetpointV2& newSetpointV2);
 
 
 
@@ -580,39 +587,39 @@ void buttonPressedWithSetpoint_4(const SetpointV2& newSetpointV2);
 // > The function that is called to "start" all estimation and control computations
 bool calculateControlOutput(Controller::Request &request, Controller::Response &response);
 
+// > The function that smooth changes in the setpoin
+void smoothSetpointChanges();
+
 // > The various functions that implement an LQR controller
-void calculateControlOutput_viaLQR(                     float stateErrorBody[12], Controller::Request &request, Controller::Response &response);
-void calculateControlOutput_viaLQRforMotors(            float stateErrorBody[12], Controller::Request &request, Controller::Response &response);
-void calculateControlOutput_viaLQRforActuators(         float stateErrorBody[12], Controller::Request &request, Controller::Response &response);
-void calculateControlOutput_viaLQRforRates(             float stateErrorBody[12], Controller::Request &request, Controller::Response &response);
-void calculateControlOutput_viaLQRforAngles(            float stateErrorBody[12], Controller::Request &request, Controller::Response &response);
-void calculateControlOutput_viaLQRforAnglesRatesNested( float stateErrorBody[12], Controller::Request &request, Controller::Response &response);
-void calculateControlOutput_viaAngleResponseTest(       float stateErrorBody[12], Controller::Request &request, Controller::Response &response);
+void calculateControlOutput_viaLQRforRates(float stateErrorBody[12], Controller::Request &request, Controller::Response &response);
 
 // ESTIMATOR COMPUTATIONS
 void performEstimatorUpdate_forStateInterial(Controller::Request &request);
 void performEstimatorUpdate_forStateInterial_viaFiniteDifference();
 void performEstimatorUpdate_forStateInterial_viaPointMassKalmanFilter();
 
-
 // PUBLISHING OF THE DEBUG MESSAGE
 void construct_and_publish_debug_message(Controller::Request &request, Controller::Response &response);
 
-
-// TRANSFORMATION FROM INTERIAL ESTIMATE TO BODY FRAME ERROR
+// TRANSFORMATION FROM INTERIAL ESTIMATE TO
+// BODY FRAME ERROR
 void convert_stateInertial_to_bodyFrameError(float stateInertial[12], float setpoint[4], float (&bodyFrameError)[12]);
 
-// TRANSFORMATION OF THE (x,y) INERTIAL FRAME ERROR INTO AN (x,y) BODY FRAME ERROR
+// TRANSFORMATION OF THE (x,y) INERTIAL FRAME ERROR
+// INTO AN (x,y) BODY FRAME ERROR
 void convertIntoBodyFrame(float stateInertial[12], float (&stateBody)[12], float yaw_measured);
 
 // CONVERSION FROM THRUST IN NEWTONS TO 16-BIT COMMAND
 float computeMotorPolyBackward(float thrust);
 
-// SETPOINT CHANGE CALLBACK
-void setpointCallback(const Setpoint& newSetpoint);
-void xyz_yaw_to_follow_callback(const Setpoint& newSetpoint);
+// REQUEST SETPOINT CHANGE CALLBACK
+void requestSetpointChangeCallback(const SetpointWithHeader& newSetpoint);
 
-void publishCurrentSetpoint();
+// CHANGE SETPOINT FUNCTION
+void setNewSetpoint(float x, float y, float z, float yaw);
+
+// GET CURRENT SETPOINT SERVICE CALLBACK
+bool getCurrentSetpointCallback(GetSetpointService::Request &request, GetSetpointService::Response &response);
 
 // CUSTOM COMMAND RECEIVED CALLBACK
 //void customCommandReceivedCallback(const CustomButton& commandReceived);
@@ -620,15 +627,18 @@ void publishCurrentSetpoint();
 // PUBLISH THE CURRENT X,Y,Z, AND YAW
 void publish_current_xyz_yaw(float x, float y, float z, float yaw);
 
-// LOAD 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 yamlReadyForFetchCallback(const std_msgs::Int32& msg);
-void fetchYamlParameters(ros::NodeHandle& nodeHandle);
-void processFetchedParameters();
+// 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();
 

pps_ws/src/d_fall_pps/include/nodes/StudentControllerService.h

@@ -249,6 +249,6 @@ bool getCurrentSetpointCallback(GetSetpointService::Request &request, GetSetpoin
 // CUSTOM COMMAND RECEIVED CALLBACK
 void customCommandReceivedCallback(const CustomButtonWithHeader& commandReceived);
 
-// > For the LOADING of YAML PARAMETERS
+// FOR LOADING THE YAML PARAMETERS
 void isReadyStudentControllerYamlCallback(const IntWithHeader & msg);
 void fetchStudentControllerYamlParameters(ros::NodeHandle& nodeHandle);
\ No newline at end of file

pps_ws/src/d_fall_pps/param/PickerController.yaml

-# Mass of the crazyflie
-mass_CF : 32
+
+# Max setpoint change per second
+max_setpoint_change_per_second_horizontal  :  0.20 # [meters]
+max_setpoint_change_per_second_vertical    :  0.10 # [meters]
+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
+#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
+#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]
+#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]
+#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
+#picker_string_length : 0.30
 
-# Max setpoint change per second
-max_setpoint_change_per_second_horizontal  :  0.20 # [meters]
-max_setpoint_change_per_second_vertical    :  0.10 # [meters]
-max_setpoint_change_per_second_yaw_degrees : 90.00 # [degrees]
-
-# Frequency of the controller, in hertz
-vicon_frequency : 200
 
 
 
@@ -40,10 +39,11 @@ vicon_frequency : 200
 
 
 
+# ------------------------------------------------------
+# PARAMTERS THAT ARE STANDARD FOR A "CONTROLLER SERVICE"
 
-
-# THE FOLLOWING PARAMETERS ARE USED
-# FOR THE LOW-LEVEL CONTROLLER
+# Mass of the crazyflie
+mass_cf_in_grams : 32
 
 # Frequency of the controller, in hertz
 control_frequency : 200
@@ -55,7 +55,7 @@ motorPoly : [5.484560e-4, 1.032633e-6, 2.130295e-11]
 shouldPerformConvertIntoBodyFrame : true
 
 # Boolean indiciating whether the (x,y,z,yaw) of this agent should be published or not
-shouldPublishCurrent_xyz_yaw : true
+#shouldPublishCurrent_xyz_yaw : true
 
 # Boolean indiciating whether the "Debug Message" of this agent should be published or not
 shouldPublishDebugMessage : true

pps_ws/src/d_fall_pps/src/nodes/StudentControllerService.cpp

@@ -962,7 +962,7 @@ int main(int argc, char* argv[]) {
 	}
 	else
 	{
-	// Inform the user
+		// Inform the user
 		ROS_ERROR("[STUDENT CONTROLLER] The request load yaml file service call failed.");
 	}
 
@@ -1011,7 +1011,7 @@ int main(int argc, char* argv[]) {
 	// an integer (that is essentially ignored), and is expected to respond
 	// with the current setpoint of the controller. When a request is made
 	// of this service the "getCurrentSetpointCallback" function is called.
-	ros::ServiceServer getCurrentSetpointService = nodeHandle.advertiseService("GetCurrentSetpoint", getCurrentSetpointCallback);	
+	ros::ServiceServer getCurrentSetpointService = nodeHandle.advertiseService("GetCurrentSetpoint", getCurrentSetpointCallback);