From e862f7ba29c966aaf344903f1ac1faf9e77435d7 Mon Sep 17 00:00:00 2001
From: beuchatp <beuchatp@control.ee.ethz.ch>
Date: Mon, 4 Feb 2019 10:21:09 +0100
Subject: [PATCH] Adjust the Picker Controller Node to be integrated with the
GUI. Needs compiling and testing.
---
.../include/nodes/PickerControllerConstants.h | 2 +-
.../include/nodes/PickerControllerService.h | 136 +-
.../include/nodes/StudentControllerService.h | 2 +-
.../d_fall_pps/param/PickerController.yaml | 46 +-
.../src/nodes/PickerControllerService.cpp | 1659 ++++++++++-------
.../src/nodes/StudentControllerService.cpp | 4 +-
6 files changed, 1056 insertions(+), 793 deletions(-)
diff --git a/pps_ws/src/d_fall_pps/include/nodes/PickerControllerConstants.h b/pps_ws/src/d_fall_pps/include/nodes/PickerControllerConstants.h
index 3b1c36cd..41ab3321 100644
--- a/pps_ws/src/d_fall_pps/include/nodes/PickerControllerConstants.h
+++ b/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
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 e1cc4c15..359438b9 100644
--- a/pps_ws/src/d_fall_pps/include/nodes/PickerControllerService.h
+++ b/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();
diff --git a/pps_ws/src/d_fall_pps/include/nodes/StudentControllerService.h b/pps_ws/src/d_fall_pps/include/nodes/StudentControllerService.h
index 42276a73..7dccc05a 100644
--- a/pps_ws/src/d_fall_pps/include/nodes/StudentControllerService.h
+++ b/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
diff --git a/pps_ws/src/d_fall_pps/param/PickerController.yaml b/pps_ws/src/d_fall_pps/param/PickerController.yaml
index 13776ed9..65e5fff4 100644
--- a/pps_ws/src/d_fall_pps/param/PickerController.yaml
+++ b/pps_ws/src/d_fall_pps/param/PickerController.yaml
@@ -1,35 +1,34 @@
-# 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
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 273d4a7f..ee97a8c3 100644
--- a/pps_ws/src/d_fall_pps/src/nodes/PickerControllerService.cpp
+++ b/pps_ws/src/d_fall_pps/src/nodes/PickerControllerService.cpp
@@ -1,4 +1,4 @@
-// Copyright (C) 2017, ETH Zurich, D-ITET, Paul Beuchat, Angel Romero, Cyrill Burgener, Marco Mueller, Philipp Friedli
+// Copyright (C) 2017, ETH Zurich, D-ITET, Paul Beuchat
//
// This file is part of D-FaLL-System.
//
@@ -89,12 +89,12 @@
// // 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 yaml_max_setpoint_change_per_second_horizontal;
+// float yaml_max_setpoint_change_per_second_vertical;
+// float yaml_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;
+// float m_control_frequency;
// A FEW EXTRA COMMENTS ABOUT THE MOST IMPORTANT VARIABLES
@@ -122,65 +122,15 @@
-// THIS FUNCTION IS CALLED AT "m_vicon_frequency" HERTZ.
+// THIS FUNCTION IS CALLED AT "m_control_frequency" HERTZ.
// IT CAN BE USED TO ADJUST THINGS IN "REAL TIME".
// For example, the equation:
-// >> m_max_setpoint_change_per_second_horizontal / m_vicon_frequency
+// >> yaml_max_setpoint_change_per_second_horizontal / m_control_frequency
// will convert the "change per second" to a "change per cycle".
void perControlCycleOperations()
{
- if (m_shouldSmoothSetpointChanges)
- {
- for(int i = 0; i < 4; ++i)
- {
- float max_for_this_coordinate;
- // FILLE IN THE STATE INERTIAL ESTIMATE TO BE USED FOR CONTROL
- switch (i)
- {
- case 0:
- max_for_this_coordinate = m_max_setpoint_change_per_second_horizontal / m_vicon_frequency;
- break;
- case 1:
- max_for_this_coordinate = m_max_setpoint_change_per_second_horizontal / m_vicon_frequency;
- break;
- case 2:
- max_for_this_coordinate = m_max_setpoint_change_per_second_vertical / m_vicon_frequency;
- break;
- case 3:
- max_for_this_coordinate = m_max_setpoint_change_per_second_yaw_radians / m_vicon_frequency;
- break;
- // Handle the exception
- default:
- max_for_this_coordinate = 0.0f;
- break;
- }
-
- // Compute the difference in setpoint
- float setpoint_difference = m_setpoint[i] - m_setpoint_for_controller[i];
-
- // Clip the difference to the maximum
- if (setpoint_difference > max_for_this_coordinate)
- {
- setpoint_difference = max_for_this_coordinate;
- }
- else if (setpoint_difference < -max_for_this_coordinate)
- {
- setpoint_difference = -max_for_this_coordinate;
- }
-
- // Update the setpoint of the controller
- m_setpoint_for_controller[i] += setpoint_difference;
- }
-
- }
- else
- {
- m_setpoint_for_controller[0] = m_setpoint[0];
- m_setpoint_for_controller[1] = m_setpoint[1];
- m_setpoint_for_controller[2] = m_setpoint[2];
- m_setpoint_for_controller[3] = m_setpoint[3];
- }
+
}
@@ -191,169 +141,182 @@ void perControlCycleOperations()
// CALLBACK FUNCTION THAT RUN WHEN THE RESPECTIVE BUTTON IS PRESSED
-void buttonPressed_gotoStart()
+// These functions are called from:
+// >> "buttonPressedWithSetpointCallback"
+// And in that function the following variable are already
+// updated appropriately:
+// >> "m_picker_current_state"
+// >> "m_mass_total_grams"
+// >> "m_shouldSmoothSetpointChanges"
+
+void buttonPressed_goto_start()
{
ROS_INFO("[PICKER CONTROLLER] Goto Start button pressed");
- // The drone should move smoothly to the start point:
- m_shouldSmoothSetpointChanges = true;
- // Set the (x,y) coordinates for the start point:
- m_setpoint[0] = m_pickup_coordinates_xy[0];
- m_setpoint[1] = m_pickup_coordinates_xy[1];
- // Set the z coordinate to be a little more than the
- // length of the "picker string"
- m_setpoint[2] = m_picker_string_length + 0.10;
- // Publish the setpoint so that the GUI updates
- publishCurrentSetpoint();
+ // // The drone should move smoothly to the start point:
+ // m_shouldSmoothSetpointChanges = true;
+ // // Set the (x,y) coordinates for the start point:
+ // m_setpoint[0] = m_pickup_coordinates_xy[0];
+ // m_setpoint[1] = m_pickup_coordinates_xy[1];
+ // // Set the z coordinate to be a little more than the
+ // // length of the "picker string"
+ // m_setpoint[2] = m_picker_string_length + 0.10;
+ // // Publish the setpoint so that the GUI updates
+ // publishCurrentSetpoint();
}
void buttonPressed_attach()
{
ROS_INFO("[PICKER CONTROLLER] Attach button pressed");
- m_shouldSmoothSetpointChanges = true;
- m_setpoint[2] = m_picker_string_length + m_thickness_of_object_at_pickup;
- publishCurrentSetpoint();
+ // m_shouldSmoothSetpointChanges = true;
+ // m_setpoint[2] = m_picker_string_length + m_thickness_of_object_at_pickup;
+ // publishCurrentSetpoint();
}
-void buttonPressed_pickup()
+void buttonPressed_lift_up()
{
ROS_INFO("[PICKER CONTROLLER] Pick up button pressed");
- m_shouldSmoothSetpointChanges = true;
- m_mass_total_grams = m_mass_CF_grams + m_mass_E_grams;
- m_setpoint[2] = m_picker_string_length + 0.25;
- publishCurrentSetpoint();
+ // m_shouldSmoothSetpointChanges = true;
+ // m_mass_total_grams = m_mass_CF_grams + m_mass_E_grams;
+ // m_setpoint[2] = m_picker_string_length + 0.25;
+ // publishCurrentSetpoint();
}
-void buttonPressed_gotoEnd()
+void buttonPressed_goto_end()
{
ROS_INFO("[PICKER CONTROLLER] Goto End button pressed");
- m_shouldSmoothSetpointChanges = true;
- m_setpoint[0] = m_dropoff_coordinates_xy_for_E[0];
- m_setpoint[1] = m_dropoff_coordinates_xy_for_E[1];
- publishCurrentSetpoint();
+ // m_shouldSmoothSetpointChanges = true;
+ // m_setpoint[0] = m_dropoff_coordinates_xy_for_E[0];
+ // m_setpoint[1] = m_dropoff_coordinates_xy_for_E[1];
+ // publishCurrentSetpoint();
}
-void buttonPressed_putdown()
+void buttonPressed_put_down()
{
ROS_INFO("[PICKER CONTROLLER] Put down button pressed");
- m_shouldSmoothSetpointChanges = true;
- m_setpoint[2] = m_picker_string_length + m_thickness_of_object_at_putdown;
- m_mass_total_grams = m_mass_CF_grams;
- publishCurrentSetpoint();
+ // m_shouldSmoothSetpointChanges = true;
+ // m_setpoint[2] = m_picker_string_length + m_thickness_of_object_at_putdown;
+ // m_mass_total_grams = m_mass_CF_grams;
+ // publishCurrentSetpoint();
}
void buttonPressed_squat()
{
ROS_INFO("[PICKER CONTROLLER] Squat button pressed");
- m_shouldSmoothSetpointChanges = true;
- m_setpoint[2] = m_picker_string_length - 0.10;
- m_mass_total_grams = m_mass_CF_grams;
- publishCurrentSetpoint();
+ // m_shouldSmoothSetpointChanges = true;
+ // m_setpoint[2] = m_picker_string_length - 0.10;
+ // m_mass_total_grams = m_mass_CF_grams;
+ // publishCurrentSetpoint();
}
void buttonPressed_jump()
{
ROS_INFO("[PICKER CONTROLLER] Jump button pressed");
- m_shouldSmoothSetpointChanges = false;
- m_setpoint[2] = m_picker_string_length + 0.10;
- m_mass_total_grams = m_mass_CF_grams;
- publishCurrentSetpoint();
+ // m_shouldSmoothSetpointChanges = false;
+ // m_setpoint[2] = m_picker_string_length + 0.10;
+ // m_mass_total_grams = m_mass_CF_grams;
+ // publishCurrentSetpoint();
}
-
-
-// THESE CALLBACK FUNCTIONS ALLOW YOU TO IMPLEMENT SOME
-// CUSTOM ACTION IN RESPONSE TO THE RESPECTIVE BUTTON PRESSES
-
-void buttonPressed_1()
-{
- ROS_INFO("[PICKER CONTROLLER] Button 1 pressed");
-}
-void buttonPressed_2()
+void buttonPressed_standby()
{
- ROS_INFO("[PICKER CONTROLLER] Button 2 pressed");
-}
+ ROS_INFO("[PICKER CONTROLLER] Standby button pressed");
-void buttonPressed_3()
-{
- ROS_INFO("[PICKER CONTROLLER] Button 3 pressed");
+ // m_shouldSmoothSetpointChanges = false;
+ // m_setpoint[2] = m_picker_string_length + 0.10;
+ // m_mass_total_grams = m_mass_CF_grams;
+ // publishCurrentSetpoint();
}
-void buttonPressed_4()
-{
- ROS_INFO("[PICKER CONTROLLER] Button 4 pressed");
-}
+// THESE CALLBACK FUNCTIONS ALLOW YOU TO IMPLEMENT SOME
+// CUSTOM ACTION IN RESPONSE TO THE RESPECTIVE BUTTON PRESSES
+// void buttonPressed_1()
+// {
+// ROS_INFO("[PICKER CONTROLLER] Button 1 pressed");
+// }
+// void buttonPressed_2()
+// {
+// ROS_INFO("[PICKER CONTROLLER] Button 2 pressed");
+// }
+// void buttonPressed_3()
+// {
+// ROS_INFO("[PICKER CONTROLLER] Button 3 pressed");
+// }
+// void buttonPressed_4()
+// {
+// ROS_INFO("[PICKER CONTROLLER] Button 4 pressed");
+// }
-void zSetpointCallback(const std_msgs::Float32& msg)
-{
- // The "data" in the message is z-height in [meters]
- float z_height = msg.data;
- // Display the data
- ROS_INFO_STREAM("[PICKER CONTROLLER] Z Slider changed to " << z_height << " [m]" );
- // Update the z-component of the setpoint class variable
- m_setpoint[2] = z_height;
-}
-void yawSetpointCallback(const std_msgs::Float32& msg)
-{
- // The "data" in the message is yaw-angle in [radians]
- float yaw_angle = msg.data;
- // Display the data
- ROS_INFO_STREAM("[PICKER CONTROLLER] Yaw Dial changed to " << (yaw_angle*RAD2DEG) << " [deg]" );
- // Update the yaw-component of the setpoint class variable
- m_setpoint[3] = yaw_angle;
-}
-void massCallback(const std_msgs::Float32& msg)
-{
- // The "data" in the message is mass in [grams]
- float mass = msg.data;
- // Display the data
- ROS_INFO_STREAM("[PICKER CONTROLLER] Mass slider changed to " << mass << " [grams]" );
- // Update the total mass class variable
- m_mass_total_grams = mass;
-}
-void xAdjustmentCallback(const std_msgs::Float32& msg)
-{
- // The "data" in the message is adjustment in [meters]
- float x_adjustment = msg.data;
- // Display the data
- ROS_INFO_STREAM("[PICKER CONTROLLER] X adjustment slider changed to " << x_adjustment << " [m]" );
- // Update the x-adjustment class variable
- m_xAdjustment = x_adjustment;
-}
-void yAdjustmentCallback(const std_msgs::Float32& msg)
-{
- // The "data" in the message is adjustment in [meters]
- float y_adjustment = msg.data;
- // Display the data
- ROS_INFO_STREAM("[PICKER CONTROLLER] Y adjustment slider changed to " << y_adjustment << " [m]" );
- // Update the y-adjustment class variable
- m_yAdjustment = y_adjustment;
-}
+// void zSetpointCallback(const std_msgs::Float32& msg)
+// {
+// // The "data" in the message is z-height in [meters]
+// float z_height = msg.data;
+// // Display the data
+// ROS_INFO_STREAM("[PICKER CONTROLLER] Z Slider changed to " << z_height << " [m]" );
+// // Update the z-component of the setpoint class variable
+// m_setpoint[2] = z_height;
+// }
+// void yawSetpointCallback(const std_msgs::Float32& msg)
+// {
+// // The "data" in the message is yaw-angle in [radians]
+// float yaw_angle = msg.data;
+// // Display the data
+// ROS_INFO_STREAM("[PICKER CONTROLLER] Yaw Dial changed to " << (yaw_angle*RAD2DEG) << " [deg]" );
+// // Update the yaw-component of the setpoint class variable
+// m_setpoint[3] = yaw_angle;
+// }
+// void massCallback(const std_msgs::Float32& msg)
+// {
+// // The "data" in the message is mass in [grams]
+// float mass = msg.data;
+// // Display the data
+// ROS_INFO_STREAM("[PICKER CONTROLLER] Mass slider changed to " << mass << " [grams]" );
+// // Update the total mass class variable
+// m_mass_total_grams = mass;
+// }
+// void xAdjustmentCallback(const std_msgs::Float32& msg)
+// {
+// // The "data" in the message is adjustment in [meters]
+// float x_adjustment = msg.data;
+// // Display the data
+// ROS_INFO_STREAM("[PICKER CONTROLLER] X adjustment slider changed to " << x_adjustment << " [m]" );
+// // Update the x-adjustment class variable
+// m_xAdjustment = x_adjustment;
+// }
+// void yAdjustmentCallback(const std_msgs::Float32& msg)
+// {
+// // The "data" in the message is adjustment in [meters]
+// float y_adjustment = msg.data;
+// // Display the data
+// ROS_INFO_STREAM("[PICKER CONTROLLER] Y adjustment slider changed to " << y_adjustment << " [m]" );
+// // Update the y-adjustment class variable
+// m_yAdjustment = y_adjustment;
+// }
@@ -363,123 +326,189 @@ 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_STREAM("[PICKER CONTROLLER] Goto Start button pressed: (x,y,z)=(" << newSetpointV2.x << "," << newSetpointV2.y << "," << newSetpointV2.z << "), smooth=" << int(newSetpointV2.isChecked) );
-
- // 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_STREAM("[PICKER CONTROLLER] Attach button pressed: z=" << newSetpointV2.z << ", smooth=" << int(newSetpointV2.isChecked) );
-
- // 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_STREAM("[PICKER CONTROLLER] Pick up button pressed: z=" << newSetpointV2.z << ", smooth=" << int(newSetpointV2.isChecked) );
-
- // 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_STREAM("[PICKER CONTROLLER] Goto End button pressed: (x,y)=(" << newSetpointV2.x << "," << newSetpointV2.y << "), smooth=" << int(newSetpointV2.isChecked) );
-
- // 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_STREAM("[PICKER CONTROLLER] Put down button pressed: z=" << newSetpointV2.z << ", smooth=" << int(newSetpointV2.isChecked) );
-
- // 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_STREAM("[PICKER CONTROLLER] Squat button pressed: z=" << newSetpointV2.z << ", smooth=" << int(newSetpointV2.isChecked) );
-
- // 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_STREAM("[PICKER CONTROLLER] Jump button pressed: z=" << newSetpointV2.z << ", smooth=" << int(newSetpointV2.isChecked) );
-
- // 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();
-}
+// CALLBACK FUNCTION THAT RUN WHEN THE RESPECTIVE BUTTON IS PRESSED
+// > AND A SETPOINT IS PROVIDED
-void buttonPressedWithSetpoint_1(const SetpointV2& newSetpointV2)
+// void buttonPressedWithSetpoint_gotoStart(const SetpointV2& newSetpointV2)
+// {
+// ROS_INFO_STREAM("[PICKER CONTROLLER] Goto Start button pressed: (x,y,z)=(" << newSetpointV2.x << "," << newSetpointV2.y << "," << newSetpointV2.z << "), smooth=" << int(newSetpointV2.isChecked) );
+
+// // 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_STREAM("[PICKER CONTROLLER] Attach button pressed: z=" << newSetpointV2.z << ", smooth=" << int(newSetpointV2.isChecked) );
+
+// // 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_STREAM("[PICKER CONTROLLER] Pick up button pressed: z=" << newSetpointV2.z << ", smooth=" << int(newSetpointV2.isChecked) );
+
+// // 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_STREAM("[PICKER CONTROLLER] Goto End button pressed: (x,y)=(" << newSetpointV2.x << "," << newSetpointV2.y << "), smooth=" << int(newSetpointV2.isChecked) );
+
+// // 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_STREAM("[PICKER CONTROLLER] Put down button pressed: z=" << newSetpointV2.z << ", smooth=" << int(newSetpointV2.isChecked) );
+
+// // 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_STREAM("[PICKER CONTROLLER] Squat button pressed: z=" << newSetpointV2.z << ", smooth=" << int(newSetpointV2.isChecked) );
+
+// // 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_STREAM("[PICKER CONTROLLER] Jump button pressed: z=" << newSetpointV2.z << ", smooth=" << int(newSetpointV2.isChecked) );
+
+// // 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");
+// }
+
+
+
+
+
+
+
+void smoothSetpointChanges()
{
- ROS_INFO("[PICKER CONTROLLER] Button 1 pressed with a setpoint provided");
-}
+ if (m_shouldSmoothSetpointChanges)
+ {
+ for(int i = 0; i < 4; ++i)
+ {
+ float max_for_this_coordinate;
+ // FILL IN THE STATE INERTIAL ESTIMATE TO BE USED FOR CONTROL
+ switch (i)
+ {
+ case 0:
+ max_for_this_coordinate = yaml_max_setpoint_change_per_second_horizontal / m_control_frequency;
+ break;
+ case 1:
+ max_for_this_coordinate = yaml_max_setpoint_change_per_second_horizontal / m_control_frequency;
+ break;
+ case 2:
+ max_for_this_coordinate = yaml_max_setpoint_change_per_second_vertical / m_control_frequency;
+ break;
+ case 3:
+ max_for_this_coordinate = m_max_setpoint_change_per_second_yaw_radians / m_control_frequency;
+ break;
+ // Handle the exception
+ default:
+ max_for_this_coordinate = 0.0f;
+ break;
+ }
-void buttonPressedWithSetpoint_2(const SetpointV2& newSetpointV2)
-{
- ROS_INFO("[PICKER CONTROLLER] Button 2 pressed with a setpoint provided");
-}
+ // Compute the difference in setpoint
+ float setpoint_difference = m_setpoint[i] - m_setpoint_for_controller[i];
-void buttonPressedWithSetpoint_3(const SetpointV2& newSetpointV2)
-{
- ROS_INFO("[PICKER CONTROLLER] Button 3 pressed with a setpoint provided");
-}
+ // Clip the difference to the maximum
+ if (setpoint_difference > max_for_this_coordinate)
+ {
+ setpoint_difference = max_for_this_coordinate;
+ }
+ else if (setpoint_difference < -max_for_this_coordinate)
+ {
+ setpoint_difference = -max_for_this_coordinate;
+ }
-void buttonPressedWithSetpoint_4(const SetpointV2& newSetpointV2)
-{
- ROS_INFO("[PICKER CONTROLLER] Button 4 pressed with a setpoint provided with a setpoint provided");
+ // Update the setpoint of the controller
+ m_setpoint_for_controller[i] += setpoint_difference;
+ }
+
+ }
+ else
+ {
+ m_setpoint_for_controller[0] = m_setpoint[0];
+ m_setpoint_for_controller[1] = m_setpoint[1];
+ m_setpoint_for_controller[2] = m_setpoint[2];
+ m_setpoint_for_controller[3] = m_setpoint[3];
+ }
}
@@ -494,10 +523,6 @@ void buttonPressedWithSetpoint_4(const SetpointV2& newSetpointV2)
-
-
-
-
// ------------------------------------------------------------------------------
// OOO U U TTTTT EEEEE RRRR
// O O U U T E R R
@@ -606,8 +631,8 @@ bool calculateControlOutput(Controller::Request &request, Controller::Response &
{
// Keep track of time
- m_time_ticks++;
- m_time_seconds = float(m_time_ticks) / m_vicon_frequency;
+ //m_time_ticks++;
+ //m_time_seconds = float(m_time_ticks) / m_control_frequency;
// CALL THE FUNCTION FOR PER CYLCE OPERATIONS
@@ -621,17 +646,21 @@ bool calculateControlOutput(Controller::Request &request, Controller::Response &
// PERFORM THE ESTIMATOR UPDATE FOR THE INTERIAL FRAME STATE
// > After this function is complete the class variable
- // "current_stateInertialEstimate" is updated and ready
+ // "m_current_stateInertialEstimate" is updated and ready
// to be used for subsequent controller copmutations
performEstimatorUpdate_forStateInterial(request);
+
+ // SMOOTH ANY CHANGES THAT MAY HAVE OCCURRED IN THE
+ // SETPOINT
+ smoothSetpointChanges();
// CONVERT THE CURRENT INERTIAL FRAME STATE ESTIMATE, INTO
// THE BODY FRAME ERROR REQUIRED BY THE CONTROLLER
// > Define a local array to fill in with the body frame error
float current_bodyFrameError[12];
// > Call the function to perform the conversion
- convert_stateInertial_to_bodyFrameError(current_stateInertialEstimate,m_setpoint_for_controller,current_bodyFrameError);
+ convert_stateInertial_to_bodyFrameError(m_current_stateInertialEstimate,m_setpoint_for_controller,current_bodyFrameError);
@@ -641,14 +670,14 @@ bool calculateControlOutput(Controller::Request &request, Controller::Response &
- // PUBLISH THE CURRENT X,Y,Z, AND YAW (if required)
- if (shouldPublishCurrent_xyz_yaw)
- {
- publish_current_xyz_yaw(request.ownCrazyflie.x,request.ownCrazyflie.y,request.ownCrazyflie.z,request.ownCrazyflie.yaw);
- }
+ // // PUBLISH THE CURRENT X,Y,Z, AND YAW (if required)
+ // if (shouldPublishCurrent_xyz_yaw)
+ // {
+ // publish_current_xyz_yaw(request.ownCrazyflie.x,request.ownCrazyflie.y,request.ownCrazyflie.z,request.ownCrazyflie.yaw);
+ // }
// PUBLISH THE DEBUG MESSAGE (if required)
- if (shouldPublishDebugMessage)
+ if (yaml_shouldPublishDebugMessage)
{
construct_and_publish_debug_message(request,response);
}
@@ -672,13 +701,13 @@ void performEstimatorUpdate_forStateInterial(Controller::Request &request)
// PUT THE CURRENT MEASURED DATA INTO THE CLASS VARIABLE
// > for (x,y,z) position
- current_xzy_rpy_measurement[0] = request.ownCrazyflie.x;
- current_xzy_rpy_measurement[1] = request.ownCrazyflie.y;
- current_xzy_rpy_measurement[2] = request.ownCrazyflie.z;
+ m_current_xzy_rpy_measurement[0] = request.ownCrazyflie.x;
+ m_current_xzy_rpy_measurement[1] = request.ownCrazyflie.y;
+ m_current_xzy_rpy_measurement[2] = request.ownCrazyflie.z;
// > for (roll,pitch,yaw) angles
- current_xzy_rpy_measurement[3] = request.ownCrazyflie.roll;
- current_xzy_rpy_measurement[4] = request.ownCrazyflie.pitch;
- current_xzy_rpy_measurement[5] = request.ownCrazyflie.yaw;
+ m_current_xzy_rpy_measurement[3] = request.ownCrazyflie.roll;
+ m_current_xzy_rpy_measurement[4] = request.ownCrazyflie.pitch;
+ m_current_xzy_rpy_measurement[5] = request.ownCrazyflie.yaw;
// RUN THE FINITE DIFFERENCE ESTIMATOR
@@ -690,7 +719,7 @@ void performEstimatorUpdate_forStateInterial(Controller::Request &request)
// FILLE IN THE STATE INERTIAL ESTIMATE TO BE USED FOR CONTROL
- switch (estimator_method)
+ switch (yaml_estimator_method)
{
// Estimator based on finte differences
case ESTIMATOR_METHOD_FINITE_DIFFERENCE:
@@ -698,7 +727,7 @@ void performEstimatorUpdate_forStateInterial(Controller::Request &request)
// Transfer the estimate
for(int i = 0; i < 12; ++i)
{
- current_stateInertialEstimate[i] = stateInterialEstimate_viaFiniteDifference[i];
+ m_current_stateInertialEstimate[i] = m_stateInterialEstimate_viaFiniteDifference[i];
}
break;
}
@@ -708,19 +737,19 @@ void performEstimatorUpdate_forStateInterial(Controller::Request &request)
// Transfer the estimate
for(int i = 0; i < 12; ++i)
{
- current_stateInertialEstimate[i] = stateInterialEstimate_viaPointMassKalmanFilter[i];
+ m_current_stateInertialEstimate[i] = m_stateInterialEstimate_viaPointMassKalmanFilter[i];
}
break;
}
// Handle the exception
default:
{
- // Display that the "estimator_method" was not recognised
- ROS_INFO_STREAM("[PICKER CONTROLLER] ERROR: in the 'calculateControlOutput' function of the 'PickerControllerService': the 'estimator_method' is not recognised.");
+ // Display that the "yaml_estimator_method" was not recognised
+ ROS_INFO_STREAM("[PICKER CONTROLLER] ERROR: in the 'calculateControlOutput' function of the 'PickerControllerService': the 'yaml_estimator_method' is not recognised.");
// Transfer the finite difference estimate by default
for(int i = 0; i < 12; ++i)
{
- current_stateInertialEstimate[i] = stateInterialEstimate_viaFiniteDifference[i];
+ m_current_stateInertialEstimate[i] = m_stateInterialEstimate_viaFiniteDifference[i];
}
break;
}
@@ -730,13 +759,13 @@ void performEstimatorUpdate_forStateInterial(Controller::Request &request)
// NOW THAT THE ESTIMATORS HAVE ALL BEEN RUN, PUT THE CURRENT
// MEASURED DATA INTO THE CLASS VARIABLE FOR THE PREVIOUS
// > for (x,y,z) position
- previous_xzy_rpy_measurement[0] = current_xzy_rpy_measurement[0];
- previous_xzy_rpy_measurement[1] = current_xzy_rpy_measurement[1];
- previous_xzy_rpy_measurement[2] = current_xzy_rpy_measurement[2];
+ m_previous_xzy_rpy_measurement[0] = m_current_xzy_rpy_measurement[0];
+ m_previous_xzy_rpy_measurement[1] = m_current_xzy_rpy_measurement[1];
+ m_previous_xzy_rpy_measurement[2] = m_current_xzy_rpy_measurement[2];
// > for (roll,pitch,yaw) angles
- previous_xzy_rpy_measurement[3] = current_xzy_rpy_measurement[3];
- previous_xzy_rpy_measurement[4] = current_xzy_rpy_measurement[4];
- previous_xzy_rpy_measurement[5] = current_xzy_rpy_measurement[5];
+ m_previous_xzy_rpy_measurement[3] = m_current_xzy_rpy_measurement[3];
+ m_previous_xzy_rpy_measurement[4] = m_current_xzy_rpy_measurement[4];
+ m_previous_xzy_rpy_measurement[5] = m_current_xzy_rpy_measurement[5];
}
@@ -745,23 +774,23 @@ void performEstimatorUpdate_forStateInterial_viaFiniteDifference()
{
// PUT IN THE CURRENT MEASUREMENT DIRECTLY
// > for (x,y,z) position
- stateInterialEstimate_viaFiniteDifference[0] = current_xzy_rpy_measurement[0];
- stateInterialEstimate_viaFiniteDifference[1] = current_xzy_rpy_measurement[1];
- stateInterialEstimate_viaFiniteDifference[2] = current_xzy_rpy_measurement[2];
+ m_stateInterialEstimate_viaFiniteDifference[0] = m_current_xzy_rpy_measurement[0];
+ m_stateInterialEstimate_viaFiniteDifference[1] = m_current_xzy_rpy_measurement[1];
+ m_stateInterialEstimate_viaFiniteDifference[2] = m_current_xzy_rpy_measurement[2];
// > for (roll,pitch,yaw) angles
- stateInterialEstimate_viaFiniteDifference[6] = current_xzy_rpy_measurement[3];
- stateInterialEstimate_viaFiniteDifference[7] = current_xzy_rpy_measurement[4];
- stateInterialEstimate_viaFiniteDifference[8] = current_xzy_rpy_measurement[5];
+ m_stateInterialEstimate_viaFiniteDifference[6] = m_current_xzy_rpy_measurement[3];
+ m_stateInterialEstimate_viaFiniteDifference[7] = m_current_xzy_rpy_measurement[4];
+ m_stateInterialEstimate_viaFiniteDifference[8] = m_current_xzy_rpy_measurement[5];
// COMPUTE THE VELOCITIES VIA FINITE DIFFERENCE
// > for (x,y,z) velocities
- stateInterialEstimate_viaFiniteDifference[3] = (current_xzy_rpy_measurement[0] - previous_xzy_rpy_measurement[0]) * estimator_frequency;
- stateInterialEstimate_viaFiniteDifference[4] = (current_xzy_rpy_measurement[1] - previous_xzy_rpy_measurement[1]) * estimator_frequency;
- stateInterialEstimate_viaFiniteDifference[5] = (current_xzy_rpy_measurement[2] - previous_xzy_rpy_measurement[2]) * estimator_frequency;
+ m_stateInterialEstimate_viaFiniteDifference[3] = (m_current_xzy_rpy_measurement[0] - m_previous_xzy_rpy_measurement[0]) * m_estimator_frequency;
+ m_stateInterialEstimate_viaFiniteDifference[4] = (m_current_xzy_rpy_measurement[1] - m_previous_xzy_rpy_measurement[1]) * m_estimator_frequency;
+ m_stateInterialEstimate_viaFiniteDifference[5] = (m_current_xzy_rpy_measurement[2] - m_previous_xzy_rpy_measurement[2]) * m_estimator_frequency;
// > for (roll,pitch,yaw) velocities
- stateInterialEstimate_viaFiniteDifference[9] = (current_xzy_rpy_measurement[3] - previous_xzy_rpy_measurement[3]) * estimator_frequency;
- stateInterialEstimate_viaFiniteDifference[10] = (current_xzy_rpy_measurement[4] - previous_xzy_rpy_measurement[4]) * estimator_frequency;
- stateInterialEstimate_viaFiniteDifference[11] = (current_xzy_rpy_measurement[5] - previous_xzy_rpy_measurement[5]) * estimator_frequency;
+ m_stateInterialEstimate_viaFiniteDifference[9] = (m_current_xzy_rpy_measurement[3] - m_previous_xzy_rpy_measurement[3]) * m_estimator_frequency;
+ m_stateInterialEstimate_viaFiniteDifference[10] = (m_current_xzy_rpy_measurement[4] - m_previous_xzy_rpy_measurement[4]) * m_estimator_frequency;
+ m_stateInterialEstimate_viaFiniteDifference[11] = (m_current_xzy_rpy_measurement[5] - m_previous_xzy_rpy_measurement[5]) * m_estimator_frequency;
}
@@ -773,28 +802,28 @@ void performEstimatorUpdate_forStateInterial_viaPointMassKalmanFilter()
float temp_PMKFstate[12];
for(int i = 0; i < 12; ++i)
{
- temp_PMKFstate[i] = stateInterialEstimate_viaPointMassKalmanFilter[i];
+ temp_PMKFstate[i] = m_stateInterialEstimate_viaPointMassKalmanFilter[i];
}
// > Now perform update for:
// > x position and velocity:
- stateInterialEstimate_viaPointMassKalmanFilter[0] = PMKF_Ahat_row1_for_positions[0]*temp_PMKFstate[0] + PMKF_Ahat_row1_for_positions[1]*temp_PMKFstate[3] + PMKF_Kinf_for_positions[0]*current_xzy_rpy_measurement[0];
- stateInterialEstimate_viaPointMassKalmanFilter[3] = PMKF_Ahat_row2_for_positions[0]*temp_PMKFstate[0] + PMKF_Ahat_row2_for_positions[1]*temp_PMKFstate[3] + PMKF_Kinf_for_positions[1]*current_xzy_rpy_measurement[0];
+ m_stateInterialEstimate_viaPointMassKalmanFilter[0] = yaml_PMKF_Ahat_row1_for_positions[0]*temp_PMKFstate[0] + yaml_PMKF_Ahat_row1_for_positions[1]*temp_PMKFstate[3] + yaml_PMKF_Kinf_for_positions[0]*m_current_xzy_rpy_measurement[0];
+ m_stateInterialEstimate_viaPointMassKalmanFilter[3] = yaml_PMKF_Ahat_row2_for_positions[0]*temp_PMKFstate[0] + yaml_PMKF_Ahat_row2_for_positions[1]*temp_PMKFstate[3] + yaml_PMKF_Kinf_for_positions[1]*m_current_xzy_rpy_measurement[0];
// > y position and velocity:
- stateInterialEstimate_viaPointMassKalmanFilter[1] = PMKF_Ahat_row1_for_positions[0]*temp_PMKFstate[1] + PMKF_Ahat_row1_for_positions[1]*temp_PMKFstate[4] + PMKF_Kinf_for_positions[0]*current_xzy_rpy_measurement[1];
- stateInterialEstimate_viaPointMassKalmanFilter[4] = PMKF_Ahat_row2_for_positions[0]*temp_PMKFstate[1] + PMKF_Ahat_row2_for_positions[1]*temp_PMKFstate[4] + PMKF_Kinf_for_positions[1]*current_xzy_rpy_measurement[1];
+ m_stateInterialEstimate_viaPointMassKalmanFilter[1] = yaml_PMKF_Ahat_row1_for_positions[0]*temp_PMKFstate[1] + yaml_PMKF_Ahat_row1_for_positions[1]*temp_PMKFstate[4] + yaml_PMKF_Kinf_for_positions[0]*m_current_xzy_rpy_measurement[1];
+ m_stateInterialEstimate_viaPointMassKalmanFilter[4] = yaml_PMKF_Ahat_row2_for_positions[0]*temp_PMKFstate[1] + yaml_PMKF_Ahat_row2_for_positions[1]*temp_PMKFstate[4] + yaml_PMKF_Kinf_for_positions[1]*m_current_xzy_rpy_measurement[1];
// > z position and velocity:
- stateInterialEstimate_viaPointMassKalmanFilter[2] = PMKF_Ahat_row1_for_positions[0]*temp_PMKFstate[2] + PMKF_Ahat_row1_for_positions[1]*temp_PMKFstate[5] + PMKF_Kinf_for_positions[0]*current_xzy_rpy_measurement[2];
- stateInterialEstimate_viaPointMassKalmanFilter[5] = PMKF_Ahat_row2_for_positions[0]*temp_PMKFstate[2] + PMKF_Ahat_row2_for_positions[1]*temp_PMKFstate[5] + PMKF_Kinf_for_positions[1]*current_xzy_rpy_measurement[2];
+ m_stateInterialEstimate_viaPointMassKalmanFilter[2] = yaml_PMKF_Ahat_row1_for_positions[0]*temp_PMKFstate[2] + yaml_PMKF_Ahat_row1_for_positions[1]*temp_PMKFstate[5] + yaml_PMKF_Kinf_for_positions[0]*m_current_xzy_rpy_measurement[2];
+ m_stateInterialEstimate_viaPointMassKalmanFilter[5] = yaml_PMKF_Ahat_row2_for_positions[0]*temp_PMKFstate[2] + yaml_PMKF_Ahat_row2_for_positions[1]*temp_PMKFstate[5] + yaml_PMKF_Kinf_for_positions[1]*m_current_xzy_rpy_measurement[2];
// > roll position and velocity:
- stateInterialEstimate_viaPointMassKalmanFilter[6] = PMKF_Ahat_row1_for_angles[0]*temp_PMKFstate[6] + PMKF_Ahat_row1_for_angles[1]*temp_PMKFstate[9] + PMKF_Kinf_for_angles[0]*current_xzy_rpy_measurement[3];
- stateInterialEstimate_viaPointMassKalmanFilter[9] = PMKF_Ahat_row2_for_angles[0]*temp_PMKFstate[6] + PMKF_Ahat_row2_for_angles[1]*temp_PMKFstate[9] + PMKF_Kinf_for_angles[1]*current_xzy_rpy_measurement[3];
+ m_stateInterialEstimate_viaPointMassKalmanFilter[6] = yaml_PMKF_Ahat_row1_for_angles[0]*temp_PMKFstate[6] + yaml_PMKF_Ahat_row1_for_angles[1]*temp_PMKFstate[9] + yaml_PMKF_Kinf_for_angles[0]*m_current_xzy_rpy_measurement[3];
+ m_stateInterialEstimate_viaPointMassKalmanFilter[9] = yaml_PMKF_Ahat_row2_for_angles[0]*temp_PMKFstate[6] + yaml_PMKF_Ahat_row2_for_angles[1]*temp_PMKFstate[9] + yaml_PMKF_Kinf_for_angles[1]*m_current_xzy_rpy_measurement[3];
// > pitch position and velocity:
- stateInterialEstimate_viaPointMassKalmanFilter[7] = PMKF_Ahat_row1_for_angles[0]*temp_PMKFstate[7] + PMKF_Ahat_row1_for_angles[1]*temp_PMKFstate[10] + PMKF_Kinf_for_angles[0]*current_xzy_rpy_measurement[4];
- stateInterialEstimate_viaPointMassKalmanFilter[10] = PMKF_Ahat_row2_for_angles[0]*temp_PMKFstate[7] + PMKF_Ahat_row2_for_angles[1]*temp_PMKFstate[10] + PMKF_Kinf_for_angles[1]*current_xzy_rpy_measurement[4];
+ m_stateInterialEstimate_viaPointMassKalmanFilter[7] = yaml_PMKF_Ahat_row1_for_angles[0]*temp_PMKFstate[7] + yaml_PMKF_Ahat_row1_for_angles[1]*temp_PMKFstate[10] + yaml_PMKF_Kinf_for_angles[0]*m_current_xzy_rpy_measurement[4];
+ m_stateInterialEstimate_viaPointMassKalmanFilter[10] = yaml_PMKF_Ahat_row2_for_angles[0]*temp_PMKFstate[7] + yaml_PMKF_Ahat_row2_for_angles[1]*temp_PMKFstate[10] + yaml_PMKF_Kinf_for_angles[1]*m_current_xzy_rpy_measurement[4];
// > yaw position and velocity:
- stateInterialEstimate_viaPointMassKalmanFilter[8] = PMKF_Ahat_row1_for_angles[0]*temp_PMKFstate[8] + PMKF_Ahat_row1_for_angles[1]*temp_PMKFstate[11] + PMKF_Kinf_for_angles[0]*current_xzy_rpy_measurement[5];
- stateInterialEstimate_viaPointMassKalmanFilter[11] = PMKF_Ahat_row2_for_angles[0]*temp_PMKFstate[8] + PMKF_Ahat_row2_for_angles[1]*temp_PMKFstate[11] + PMKF_Kinf_for_angles[1]*current_xzy_rpy_measurement[5];
+ m_stateInterialEstimate_viaPointMassKalmanFilter[8] = yaml_PMKF_Ahat_row1_for_angles[0]*temp_PMKFstate[8] + yaml_PMKF_Ahat_row1_for_angles[1]*temp_PMKFstate[11] + yaml_PMKF_Kinf_for_angles[0]*m_current_xzy_rpy_measurement[5];
+ m_stateInterialEstimate_viaPointMassKalmanFilter[11] = yaml_PMKF_Ahat_row2_for_angles[0]*temp_PMKFstate[8] + yaml_PMKF_Ahat_row2_for_angles[1]*temp_PMKFstate[11] + yaml_PMKF_Kinf_for_angles[1]*m_current_xzy_rpy_measurement[5];
}
@@ -828,13 +857,13 @@ void calculateControlOutput_viaLQRforRates(float stateErrorBody[12], Controller:
for(int i = 0; i < 9; ++i)
{
// BODY FRAME Y CONTROLLER
- rollRate_forResponse -= gainMatrixRollRate[i] * stateErrorBody[i];
+ rollRate_forResponse -= yaml_gainMatrixRollRate[i] * stateErrorBody[i];
// BODY FRAME X CONTROLLER
- pitchRate_forResponse -= gainMatrixPitchRate[i] * stateErrorBody[i];
+ pitchRate_forResponse -= yaml_gainMatrixPitchRate[i] * stateErrorBody[i];
// BODY FRAME YAW CONTROLLER
- yawRate_forResponse -= gainMatrixYawRate[i] * stateErrorBody[i];
+ yawRate_forResponse -= yaml_gainMatrixYawRate[i] * stateErrorBody[i];
// > ALITUDE CONTROLLER (i.e., z-controller):
- thrustAdjustment -= gainMatrixThrust_NineStateVector[i] * stateErrorBody[i];
+ thrustAdjustment -= yaml_gainMatrixThrust_NineStateVector[i] * stateErrorBody[i];
}
@@ -851,7 +880,7 @@ void calculateControlOutput_viaLQRforRates(float stateErrorBody[12], Controller:
// controller needed to be divided by 4 or not.
thrustAdjustment = thrustAdjustment / 4.0;
// > Compute the feed-forward force
- float feed_forward_thrust_per_motor = m_mass_total_grams * 9.81/(1000*4);
+ float feed_forward_thrust_per_motor = m_weight_total_in_newtons / 4.0;
// > Put in the per motor commands
response.controlOutput.motorCmd1 = computeMotorPolyBackward(thrustAdjustment + feed_forward_thrust_per_motor);
response.controlOutput.motorCmd2 = computeMotorPolyBackward(thrustAdjustment + feed_forward_thrust_per_motor);
@@ -867,7 +896,7 @@ void calculateControlOutput_viaLQRforRates(float stateErrorBody[12], Controller:
// An alternate debugging technique is to print out data directly to the
// command line from which this node was launched.
- if (shouldDisplayDebugInfo)
+ if (yaml_shouldDisplayDebugInfo)
{
// An example of "printing out" the data from the "request" argument to the
// command line. This might be useful for debugging.
@@ -886,9 +915,9 @@ void calculateControlOutput_viaLQRforRates(float stateErrorBody[12], Controller:
ROS_INFO_STREAM("controlOutput.yaw = " << response.controlOutput.yaw);
// An example of "printing out" the "thrust-to-command" conversion parameters.
- ROS_INFO_STREAM("motorPoly 0:" << motorPoly[0]);
- ROS_INFO_STREAM("motorPoly 0:" << motorPoly[1]);
- ROS_INFO_STREAM("motorPoly 0:" << motorPoly[2]);
+ ROS_INFO_STREAM("motorPoly 0:" << yaml_motorPoly[0]);
+ ROS_INFO_STREAM("motorPoly 1:" << yaml_motorPoly[1]);
+ ROS_INFO_STREAM("motorPoly 2:" << yaml_motorPoly[2]);
// An example of "printing out" the per motor 16-bit command computed.
ROS_INFO_STREAM("controlOutput.cmd1 = " << response.controlOutput.motorCmd1);
@@ -939,7 +968,7 @@ void construct_and_publish_debug_message(Controller::Request &request, Controlle
// debugMsg.value_10 = your_variable_name;
// Publish the "debugMsg"
- debugPublisher.publish(debugMsg);
+ m_debugPublisher.publish(debugMsg);
}
@@ -959,7 +988,7 @@ void construct_and_publish_debug_message(Controller::Request &request, Controlle
void convertIntoBodyFrame(float stateInertial[12], float (&stateBody)[12], float yaw_measured)
{
- if (shouldPerformConvertIntoBodyFrame)
+ if (yaml_shouldPerformConvertIntoBodyFrame)
{
float sinYaw = sin(yaw_measured);
float cosYaw = cos(yaw_measured);
@@ -1017,17 +1046,18 @@ void convert_stateInertial_to_bodyFrameError(float stateInertial[12], float setp
float temp_stateInertial_yaw = stateInertial[8];
// Adjust the INERTIAL (x,y,z) position for the setpoint
- stateInertial[0] = stateInertial[0] - setpoint[0] - m_xAdjustment;
- stateInertial[1] = stateInertial[1] - setpoint[1] - m_yAdjustment;
+ stateInertial[0] = stateInertial[0] - setpoint[0];
+ stateInertial[1] = stateInertial[1] - setpoint[1];
stateInertial[2] = stateInertial[2] - setpoint[2];
- if (stateInertial[2] > 40.0f)
+ // Clip the z-coordination to +/-0.40 meters
+ if (stateInertial[2] > 0.40f)
{
- stateInertial[2] = 40.0f;
+ stateInertial[2] = 0.40f;
}
- else if (stateInertial[2] < -40.0f)
+ else if (stateInertial[2] < -0.40f)
{
- stateInertial[2] = -40.0f;
+ stateInertial[2] = -0.40f;
}
// Fill in the yaw angle error
@@ -1081,16 +1111,16 @@ void convert_stateInertial_to_bodyFrameError(float stateInertial[12], float setp
float computeMotorPolyBackward(float thrust)
{
// Compute the 16-bit command signal that generates the "thrust" force
- float cmd = (-motorPoly[1] + sqrt(motorPoly[1] * motorPoly[1] - 4 * motorPoly[2] * (motorPoly[0] - thrust))) / (2 * motorPoly[2]);
+ float cmd = (-yaml_motorPoly[1] + sqrt(yaml_motorPoly[1] * yaml_motorPoly[1] - 4 * yaml_motorPoly[2] * (yaml_motorPoly[0] - thrust))) / (2 * yaml_motorPoly[2]);
// Saturate the signal to be 0 or in the range [1000,65000]
- if (cmd < cmd_sixteenbit_min)
+ if (cmd < yaml_cmd_sixteenbit_min)
{
cmd = 0;
}
- else if (cmd > cmd_sixteenbit_max)
+ else if (cmd > yaml_cmd_sixteenbit_max)
{
- cmd = cmd_sixteenbit_max;
+ cmd = yaml_cmd_sixteenbit_max;
}
return cmd;
@@ -1113,28 +1143,184 @@ float computeMotorPolyBackward(float thrust)
// GGGG UUU III CCCC A A LLLLL LLLLL BBBB A A CCCC K K
// ----------------------------------------------------------------------------------
-// This function DOES NOT NEED TO BE edited for successful completion of the PPS exercise
-void setpointCallback(const Setpoint& newSetpoint)
+
+// REQUEST SETPOINT CHANGE CALLBACK
+// This function does NOT need to be edited
+void requestSetpointChangeCallback(const SetpointWithHeader& newSetpoint)
{
- m_setpoint[0] = newSetpoint.x;
- m_setpoint[1] = newSetpoint.y;
- m_setpoint[2] = newSetpoint.z;
- m_setpoint[3] = newSetpoint.yaw;
+ // Check whether the message is relevant
+ bool isRevelant = checkMessageHeader( m_agentID , newSetpoint.shouldCheckForAgentID , newSetpoint.agentIDs );
+
+ // Continue if the message is relevant
+ if (isRevelant)
+ {
+ // Check that the ".buttonID" property is
+ // actually one of the possible states
+ int button_index = newSetpoint.buttonID;
+ switch(button_index)
+ {
+ case PICKER_STATE_STANDBY:
+ case PICKER_STATE_GOTO_START:
+ case PICKER_STATE_ATTACH:
+ case PICKER_STATE_LIFT_UP:
+ case PICKER_STATE_GOTO_END:
+ case PICKER_STATE_PUT_DOWN:
+ case PICKER_STATE_SQUAT:
+ case PICKER_STATE_JUMP:
+ {
+ // Call the function for actually setting the setpoint
+ setNewSetpoint(
+ newSetpoint.buttonID,
+ newSetpoint.isChecked,
+ newSetpoint.x,
+ newSetpoint.y,
+ newSetpoint.z,
+ newSetpoint.yaw,
+ newSetpoint.mass
+ );
+ 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;
+ }
+ }
+
+ // Call the specific function for each button
+ switch(button_index)
+ {
+ case PICKER_STATE_STANDBY:
+ {
+ buttonPressed_standby();
+ break;
+ }
+ case PICKER_STATE_GOTO_START:
+ {
+ buttonPressed_goto_start();
+ break;
+ }
+ case PICKER_STATE_ATTACH:
+ {
+ buttonPressed_attach();
+ break;
+ }
+ case PICKER_STATE_LIFT_UP:
+ {
+ buttonPressed_lift_up();
+ break;
+ }
+ case PICKER_STATE_GOTO_END:
+ {
+ buttonPressed_goto_end();
+ break;
+ }
+ case PICKER_STATE_PUT_DOWN:
+ {
+ buttonPressed_put_down();
+ break;
+ }
+ case PICKER_STATE_SQUAT:
+ {
+ buttonPressed_squat();
+ break;
+ }
+ case PICKER_STATE_JUMP:
+ {
+ buttonPressed_jump();
+ break;
+ }
+ }
+ }
}
-void publishCurrentSetpoint()
+// CHANGE SETPOINT FUNCTION
+// This function does NOT need to be edited
+void setNewSetpoint(int state, bool should_smooth, float x, float y, float z, float yaw, float mass)
{
- 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];
+ // Put the state into the class variable
+ m_picker_current_state = state;
+
+ // Put the smoothing flag in the class variable
+ m_shouldSmoothSetpointChanges = should_smooth;
+
+ // Put the new setpoint into the class variable
+ m_setpoint[0] = x;
+ m_setpoint[1] = y;
+ m_setpoint[2] = z;
+ m_setpoint[3] = yaw;
+
+ // Put the mass into the class variable
+ m_mass_total_in_grams = mass;
+ m_weight_total_in_newtons = m_mass_total_in_grams * (9.81/1000.0);
+
+ // Publish the change so that the network is updated
+ // (mainly the "flying agent GUI" is interested in
+ // displaying this change to the user)
+
+ // Instantiate a local variable of type "SetpointWithHeader"
+ SetpointWithHeader msg;
+ // Fill in the setpoint
+ msg.buttonID = state;
+ msg.isChecked = should_smooth;
+ msg.x = x;
+ msg.y = y;
+ msg.z = z;
+ msg.yaw = yaw;
+ msg.mass = mass;
+ // Publish the message
+ m_setpointChangedPublisher.publish(msg);
+}
- pickerSetpointToGUIPublisher.publish(msg_setpoint);
+
+// GET CURRENT SETPOINT SERVICE CALLBACK
+// This function does NOT need to be edited
+bool getCurrentSetpointCallback(GetSetpointService::Request &request, GetSetpointService::Response &response)
+{
+ // Directly put the current setpoint into the response
+ response.setpointWithHeader.buttonID = m_picker_current_state;
+ response.setpointWithHeader.isChecked = m_shouldSmoothSetpointChanges;
+ response.setpointWithHeader.x = m_setpoint[0];
+ response.setpointWithHeader.y = m_setpoint[1];
+ response.setpointWithHeader.z = m_setpoint[2];
+ response.setpointWithHeader.yaw = m_setpoint[3];
+ response.setpointWithHeader.mass = m_mass_total_in_grams;
+ // Return
+ return true;
}
+
+
+
+
+// // This function DOES NOT NEED TO BE edited for successful completion of the PPS exercise
+// void setpointCallback(const Setpoint& newSetpoint)
+// {
+// m_setpoint[0] = newSetpoint.x;
+// m_setpoint[1] = newSetpoint.y;
+// m_setpoint[2] = newSetpoint.z;
+// m_setpoint[3] = newSetpoint.yaw;
+// }
+
+
+// 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
// C U U S T O O MM MM
@@ -1150,107 +1336,107 @@ void publishCurrentSetpoint()
// ----------------------------------------------------------------------------------
// CUSTOM COMMAND RECEIVED CALLBACK
-void buttonPressedCallback(const std_msgs::Int32& msg)
-{
- // Extract the data from the message
- int button_index = msg.data;
-
- // Switch between the button pressed
- switch(button_index)
- {
- case PICKER_BUTTON_GOTOSTART:
- buttonPressed_gotoStart();
- break;
- case PICKER_BUTTON_ATTACH:
- buttonPressed_attach();
- break;
- case PICKER_BUTTON_PICKUP:
- buttonPressed_pickup();
- break;
- case PICKER_BUTTON_GOTOEND:
- buttonPressed_gotoEnd();
- break;
- case PICKER_BUTTON_PUTDOWN:
- buttonPressed_putdown();
- break;
- case PICKER_BUTTON_SQUAT:
- buttonPressed_squat();
- break;
- case PICKER_BUTTON_JUMP:
- buttonPressed_jump();
- break;
- case PICKER_BUTTON_1:
- buttonPressed_1();
- break;
- case PICKER_BUTTON_2:
- buttonPressed_2();
- break;
- case PICKER_BUTTON_3:
- buttonPressed_3();
- break;
- case PICKER_BUTTON_4:
- buttonPressed_4();
- 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;
- }
- }
-}
-
-
-
-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;
- }
- }
-}
+// void buttonPressedCallback(const std_msgs::Int32& msg)
+// {
+// // Extract the data from the message
+// int button_index = msg.data;
+
+// // Switch between the button pressed
+// switch(button_index)
+// {
+// case PICKER_BUTTON_GOTOSTART:
+// buttonPressed_gotoStart();
+// break;
+// case PICKER_BUTTON_ATTACH:
+// buttonPressed_attach();
+// break;
+// case PICKER_BUTTON_PICKUP:
+// buttonPressed_pickup();
+// break;
+// case PICKER_BUTTON_GOTOEND:
+// buttonPressed_gotoEnd();
+// break;
+// case PICKER_BUTTON_PUTDOWN:
+// buttonPressed_putdown();
+// break;
+// case PICKER_BUTTON_SQUAT:
+// buttonPressed_squat();
+// break;
+// case PICKER_BUTTON_JUMP:
+// buttonPressed_jump();
+// break;
+// case PICKER_BUTTON_1:
+// buttonPressed_1();
+// break;
+// case PICKER_BUTTON_2:
+// buttonPressed_2();
+// break;
+// case PICKER_BUTTON_3:
+// buttonPressed_3();
+// break;
+// case PICKER_BUTTON_4:
+// buttonPressed_4();
+// 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;
+// }
+// }
+// }
+
+
+
+// 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;
+// }
+// }
+// }
@@ -1294,168 +1480,156 @@ void publish_current_xyz_yaw(float x, float y, float z, float yaw)
// ----------------------------------------------------------------------------------
-// This function DOES NOT NEED TO BE edited for successful completion of the PPS exercise
-void yamlReadyForFetchCallback(const std_msgs::Int32& msg)
+// LOADING OF YAML PARAMETERS
+void isReadyPickerControllerYamlCallback(const IntWithHeader & msg)
{
- // Extract from the "msg" for which controller the and from where to fetch the YAML
- // parameters
- int controller_to_fetch_yaml = msg.data;
+ // Check whether the message is relevant
+ bool isRevelant = checkMessageHeader( m_agentID , msg.shouldCheckForID , msg.agentIDs );
- // Switch between fetching for the different controllers and from different locations
- switch(controller_to_fetch_yaml)
+ // Continue if the message is relevant
+ if (isRevelant)
{
-
- // > FOR FETCHING FROM THE AGENT'S OWN PARAMETER SERVICE
- case FETCH_YAML_PICKER_CONTROLLER_FROM_OWN_AGENT:
- {
- // Let the user know that this message was received
- ROS_INFO("[PICKER CONTROLLER] Received the message that YAML parameters were (re-)loaded. > Now fetching the parameter values from this agent.");
- // Create a node handle to the parameter service running on this agent's machine
- ros::NodeHandle nodeHandle_to_own_agent_parameter_service(namespace_to_own_agent_parameter_service);
- // Call the function that fetches the parameters
- fetchYamlParameters(nodeHandle_to_own_agent_parameter_service);
- break;
- }
-
- // > FOR FETCHING FROM THE COORDINATOR'S PARAMETER SERVICE
- case FETCH_YAML_PICKER_CONTROLLER_FROM_COORDINATOR:
+ // Extract the data
+ int parameter_service_to_load_from = msg.data;
+ // Initialise a local variable for the namespace
+ std::string namespace_to_use;
+ // Load from the respective parameter service
+ switch(parameter_service_to_load_from)
{
- // Let the user know that this message was received
- ROS_INFO("[PICKER CONTROLLER] Received the message that YAML parameters were (re-)loaded. > Now fetching the parameter values from the coordinator.");
- // Create a node handle to the parameter service running on the coordinator machine
- ros::NodeHandle nodeHandle_to_coordinator_parameter_service(namespace_to_coordinator_parameter_service);
- // Call the function that fetches the parameters
- fetchYamlParameters(nodeHandle_to_coordinator_parameter_service);
- break;
- }
+ // > FOR FETCHING FROM THE AGENT'S OWN PARAMETER SERVICE
+ case LOAD_YAML_FROM_AGENT:
+ {
+ ROS_INFO("[PICKER CONTROLLER] Now fetching the PickerController YAML parameter values from this agent.");
+ namespace_to_use = m_namespace_to_own_agent_parameter_service;
+ break;
+ }
+ // > FOR FETCHING FROM THE COORDINATOR'S PARAMETER SERVICE
+ case LOAD_YAML_FROM_COORDINATOR:
+ {
+ ROS_INFO("[PICKER CONTROLLER] Now fetching the PickerController YAML parameter values from this agent's coordinator.");
+ namespace_to_use = m_namespace_to_coordinator_parameter_service;
+ break;
+ }
- default:
- {
- // Let the user know that the command was not relevant
- //ROS_INFO("The PickerControllerService received the message that YAML parameters were (re-)loaded");
- //ROS_INFO("> However the parameters do not relate to this controller, hence nothing will be fetched.");
- break;
+ default:
+ {
+ ROS_ERROR("[PICKER CONTROLLER] Paramter service to load from was NOT recognised.");
+ namespace_to_use = m_namespace_to_own_agent_parameter_service;
+ break;
+ }
}
+ // Create a node handle to the selected parameter service
+ ros::NodeHandle nodeHandle_to_use(namespace_to_use);
+ // Call the function that fetches the parameters
+ fetchPickerControllerYamlParameters(nodeHandle_to_use);
}
}
-// This function CAN BE edited for successful completion of the PPS exercise, and the
-// use of parameters fetched from the YAML file is highly recommended to make tuning of
-// your controller easier and quicker.
-void fetchYamlParameters(ros::NodeHandle& nodeHandle)
+void fetchPickerControllerYamlParameters(ros::NodeHandle& nodeHandle)
{
- // Here we load the parameters that are specified in the PickerController.yaml file
+ // Here we load the parameters that are specified in the file:
+ // StudentController.yaml
// Add the "PickerController" namespace to the "nodeHandle"
- ros::NodeHandle nodeHandle_for_pickerController(nodeHandle, "PickerController");
+ ros::NodeHandle nodeHandle_for_paramaters(nodeHandle, "PickerController");
+
- // > The mass of the crazyflie
- m_mass_CF_grams = getParameterFloat(nodeHandle_for_pickerController , "mass_CF");
- // > The mass of the letters
- m_mass_E_grams = getParameterFloat(nodeHandle_for_pickerController , "mass_E");
- m_mass_T_grams = getParameterFloat(nodeHandle_for_pickerController , "mass_T");
- m_mass_H_grams = getParameterFloat(nodeHandle_for_pickerController , "mass_H");
+ // GET THE PARAMETERS:
- // 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
- m_thickness_of_object_at_pickup = getParameterFloat(nodeHandle_for_pickerController , "thickness_of_object_at_pickup");
- m_thickness_of_object_at_putdown = getParameterFloat(nodeHandle_for_pickerController , "thickness_of_object_at_putdown");
+ // // // > The mass of the letters
+ // m_mass_E_grams = getParameterFloat(nodeHandle_for_pickerController , "mass_E");
+ // m_mass_T_grams = getParameterFloat(nodeHandle_for_pickerController , "mass_T");
+ // m_mass_H_grams = getParameterFloat(nodeHandle_for_pickerController , "mass_H");
- // (x,y) coordinates of the pickup location
- getParameterFloatVector(nodeHandle_for_pickerController, "pickup_coordinates_xy", m_pickup_coordinates_xy, 2);
+ // // 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
+ // m_thickness_of_object_at_pickup = getParameterFloat(nodeHandle_for_pickerController , "thickness_of_object_at_pickup");
+ // m_thickness_of_object_at_putdown = getParameterFloat(nodeHandle_for_pickerController , "thickness_of_object_at_putdown");
- // (x,y) coordinates of the drop off location
- getParameterFloatVector(nodeHandle_for_pickerController, "dropoff_coordinates_xy_for_E", m_dropoff_coordinates_xy_for_E, 2);
- 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);
+ // // (x,y) coordinates of the pickup location
+ // getParameterFloatVector(nodeHandle_for_pickerController, "pickup_coordinates_xy", m_pickup_coordinates_xy, 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");
+ // // (x,y) coordinates of the drop off location
+ // getParameterFloatVector(nodeHandle_for_pickerController, "dropoff_coordinates_xy_for_E", m_dropoff_coordinates_xy_for_E, 2);
+ // 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");
// Max setpoint change per second
- m_max_setpoint_change_per_second_horizontal = getParameterFloat(nodeHandle_for_pickerController , "max_setpoint_change_per_second_horizontal");
- m_max_setpoint_change_per_second_vertical = getParameterFloat(nodeHandle_for_pickerController , "max_setpoint_change_per_second_vertical");
- m_max_setpoint_change_per_second_yaw_degrees = getParameterFloat(nodeHandle_for_pickerController , "max_setpoint_change_per_second_yaw_degrees");
+ yaml_max_setpoint_change_per_second_horizontal = getParameterFloat(nodeHandle_for_pickerController , "max_setpoint_change_per_second_horizontal");
+ yaml_max_setpoint_change_per_second_vertical = getParameterFloat(nodeHandle_for_pickerController , "max_setpoint_change_per_second_vertical");
+ yaml_max_setpoint_change_per_second_yaw_degrees = getParameterFloat(nodeHandle_for_pickerController , "max_setpoint_change_per_second_yaw_degrees");
+
+
- // THE FOLLOWING PARAMETERS ARE USED FOR THE LOW-LEVEL CONTROLLER
+ // ------------------------------------------------------
+ // PARAMTERS THAT ARE STANDARD FOR A "CONTROLLER SERVICE"
- // > The frequency at which the "computeControlOutput" is being called, as determined
- // by the frequency at which the Vicon system provides position and attitude data
- m_vicon_frequency = getParameterFloat(nodeHandle_for_pickerController, "vicon_frequency");
+ // > The mass of the crazyflie
+ yaml_mass_cf_in_grams = getParameterFloat(nodeHandle_for_pickerController , "mass_cf_in_grams");
// > The frequency at which the "computeControlOutput" is being called, as determined
// by the frequency at which the Vicon system provides position and attitude data
- control_frequency = getParameterFloat(nodeHandle_for_pickerController, "control_frequency");
+ yaml_control_frequency = getParameterFloat(nodeHandle_for_pickerController, "control_frequency");
// > The co-efficients of the quadratic conversation from 16-bit motor command to
// thrust force in Newtons
- getParameterFloatVector(nodeHandle_for_pickerController, "motorPoly", motorPoly, 3);
+ getParameterFloatVector(nodeHandle_for_pickerController, "motorPoly", yaml_motorPoly, 3);
// > The boolean for whether to execute the convert into body frame function
- shouldPerformConvertIntoBodyFrame = getParameterBool(nodeHandle_for_pickerController, "shouldPerformConvertIntoBodyFrame");
+ yaml_shouldPerformConvertIntoBodyFrame = getParameterBool(nodeHandle_for_pickerController, "shouldPerformConvertIntoBodyFrame");
- // > 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");
+ // // > 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");
// Boolean indiciating whether the "Debug Message" of this agent should be published or not
- shouldPublishDebugMessage = getParameterBool(nodeHandle_for_pickerController, "shouldPublishDebugMessage");
+ yaml_shouldPublishDebugMessage = getParameterBool(nodeHandle_for_pickerController, "shouldPublishDebugMessage");
// Boolean indiciating whether the debugging ROS_INFO_STREAM should be displayed or not
- shouldDisplayDebugInfo = getParameterBool(nodeHandle_for_pickerController, "shouldDisplayDebugInfo");
-
-
- // THE CONTROLLER GAINS:
+ yaml_shouldDisplayDebugInfo = getParameterBool(nodeHandle_for_pickerController, "shouldDisplayDebugInfo");
// A flag for which estimator to use:
- estimator_method = getParameterInt( nodeHandle_for_pickerController , "estimator_method" );
+ yaml_estimator_method = getParameterInt( nodeHandle_for_pickerController , "estimator_method" );
// The LQR Controller parameters for "LQR_MODE_RATE"
- getParameterFloatVector(nodeHandle_for_pickerController, "gainMatrixThrust_NineStateVector", gainMatrixThrust_NineStateVector, 9);
- getParameterFloatVector(nodeHandle_for_pickerController, "gainMatrixRollRate", gainMatrixRollRate, 9);
- getParameterFloatVector(nodeHandle_for_pickerController, "gainMatrixPitchRate", gainMatrixPitchRate, 9);
- getParameterFloatVector(nodeHandle_for_pickerController, "gainMatrixYawRate", gainMatrixYawRate, 9);
+ getParameterFloatVector(nodeHandle_for_pickerController, "gainMatrixThrust_NineStateVector", yaml_gainMatrixThrust_NineStateVector, 9);
+ getParameterFloatVector(nodeHandle_for_pickerController, "gainMatrixRollRate", yaml_gainMatrixRollRate, 9);
+ getParameterFloatVector(nodeHandle_for_pickerController, "gainMatrixPitchRate", yaml_gainMatrixPitchRate, 9);
+ getParameterFloatVector(nodeHandle_for_pickerController, "gainMatrixYawRate", yaml_gainMatrixYawRate, 9);
// 16-BIT COMMAND LIMITS
- cmd_sixteenbit_min = getParameterFloat(nodeHandle_for_pickerController, "command_sixteenbit_min");
- cmd_sixteenbit_max = getParameterFloat(nodeHandle_for_pickerController, "command_sixteenbit_max");
-
+ yaml_cmd_sixteenbit_min = getParameterFloat(nodeHandle_for_pickerController, "command_sixteenbit_min");
+ yaml_cmd_sixteenbit_max = getParameterFloat(nodeHandle_for_pickerController, "command_sixteenbit_max");
// THE POINT MASS KALMAN FILTER (PMKF) GAINS AND ERROR EVOLUATION
// > For the (x,y,z) position
- getParameterFloatVector(nodeHandle_for_pickerController, "PMKF_Ahat_row1_for_positions", PMKF_Ahat_row1_for_positions, 2);
- getParameterFloatVector(nodeHandle_for_pickerController, "PMKF_Ahat_row2_for_positions", PMKF_Ahat_row2_for_positions, 2);
- getParameterFloatVector(nodeHandle_for_pickerController, "PMKF_Kinf_for_positions" , PMKF_Kinf_for_positions , 2);
+ getParameterFloatVector(nodeHandle_for_pickerController, "PMKF_Ahat_row1_for_positions", yaml_PMKF_Ahat_row1_for_positions, 2);
+ getParameterFloatVector(nodeHandle_for_pickerController, "PMKF_Ahat_row2_for_positions", yaml_PMKF_Ahat_row2_for_positions, 2);
+ getParameterFloatVector(nodeHandle_for_pickerController, "PMKF_Kinf_for_positions" , yaml_PMKF_Kinf_for_positions , 2);
// > For the (roll,pitch,yaw) angles
- getParameterFloatVector(nodeHandle_for_pickerController, "PMKF_Ahat_row1_for_angles", PMKF_Ahat_row1_for_angles, 2);
- getParameterFloatVector(nodeHandle_for_pickerController, "PMKF_Ahat_row2_for_angles", PMKF_Ahat_row2_for_angles, 2);
- getParameterFloatVector(nodeHandle_for_pickerController, "PMKF_Kinf_for_angles" , PMKF_Kinf_for_angles , 2);
+ getParameterFloatVector(nodeHandle_for_pickerController, "PMKF_Ahat_row1_for_angles", yaml_PMKF_Ahat_row1_for_angles, 2);
+ getParameterFloatVector(nodeHandle_for_pickerController, "PMKF_Ahat_row2_for_angles", yaml_PMKF_Ahat_row2_for_angles, 2);
+ getParameterFloatVector(nodeHandle_for_pickerController, "PMKF_Kinf_for_angles" , yaml_PMKF_Kinf_for_angles , 2);
// DEBUGGING: Print out one of the parameters that was loaded
- ROS_INFO_STREAM("[PICKER CONTROLLER] DEBUGGING: the fetched PickerController/mass_CF = " << m_mass_CF_grams);
+ ROS_INFO_STREAM("[PICKER CONTROLLER] DEBUGGING: the fetched PickerController/mass_cf_in_grams = " << yaml_mass_cf_in_grams);
- // Call the function that computes details an values that are needed from these
- // parameters loaded above
- processFetchedParameters();
-}
-
-// This function CAN BE edited for successful completion of the PPS exercise, and the
-// use of parameters loaded from the YAML file is highly recommended to make tuning of
-// your controller easier and quicker.
-void processFetchedParameters()
-{
+ // PROCESS THE PARAMTERS
// Convert from degrees to radians
- m_max_setpoint_change_per_second_yaw_radians = DEG2RAD * m_max_setpoint_change_per_second_yaw_degrees;
+ m_max_setpoint_change_per_second_yaw_radians = DEG2RAD * yaml_max_setpoint_change_per_second_yaw_degrees;
// Set that the estimator frequency is the same as the control frequency
- estimator_frequency = m_vicon_frequency;
-}
+ m_estimator_frequency = yaml_control_frequency;
+}
@@ -1468,64 +1642,64 @@ void processFetchedParameters()
// ----------------------------------------------------------------------------------
-// This function DOES NOT NEED TO BE edited for successful completion of the PPS exercise
-float getParameterFloat(ros::NodeHandle& nodeHandle, std::string name)
-{
- float val;
- if(!nodeHandle.getParam(name, val))
- {
- ROS_ERROR_STREAM("missing parameter '" << name << "'");
- }
- return val;
-}
-// This function DOES NOT NEED TO BE edited for successful completion of the PPS exercise
-void getParameterFloatVector(ros::NodeHandle& nodeHandle, std::string name, std::vector<float>& val, int length)
-{
- if(!nodeHandle.getParam(name, val)){
- ROS_ERROR_STREAM("missing parameter '" << name << "'");
- }
- if(val.size() != length) {
- ROS_ERROR_STREAM("parameter '" << name << "' has wrong array length, " << length << " needed");
- }
-}
-// This function DOES NOT NEED TO BE edited for successful completion of the PPS exercise
-int getParameterInt(ros::NodeHandle& nodeHandle, std::string name)
-{
- int val;
- if(!nodeHandle.getParam(name, val))
- {
- ROS_ERROR_STREAM("missing parameter '" << name << "'");
- }
- return val;
-}
-// This function DOES NOT NEED TO BE edited for successful completion of the PPS exercise
-void getParameterIntVectorWithKnownLength(ros::NodeHandle& nodeHandle, std::string name, std::vector<int>& val, int length)
-{
- if(!nodeHandle.getParam(name, val)){
- ROS_ERROR_STREAM("missing parameter '" << name << "'");
- }
- if(val.size() != length) {
- ROS_ERROR_STREAM("parameter '" << name << "' has wrong array length, " << length << " needed");
- }
-}
-// This function DOES NOT NEED TO BE edited for successful completion of the PPS exercise
-int getParameterIntVectorWithUnknownLength(ros::NodeHandle& nodeHandle, std::string name, std::vector<int>& val)
-{
- if(!nodeHandle.getParam(name, val)){
- ROS_ERROR_STREAM("missing parameter '" << name << "'");
- }
- return val.size();
-}
-// This function DOES NOT NEED TO BE edited for successful completion of the PPS exercise
-bool getParameterBool(ros::NodeHandle& nodeHandle, std::string name)
-{
- bool val;
- if(!nodeHandle.getParam(name, val))
- {
- ROS_ERROR_STREAM("missing parameter '" << name << "'");
- }
- return val;
-}
+// // This function DOES NOT NEED TO BE edited for successful completion of the PPS exercise
+// float getParameterFloat(ros::NodeHandle& nodeHandle, std::string name)
+// {
+// float val;
+// if(!nodeHandle.getParam(name, val))
+// {
+// ROS_ERROR_STREAM("missing parameter '" << name << "'");
+// }
+// return val;
+// }
+// // This function DOES NOT NEED TO BE edited for successful completion of the PPS exercise
+// void getParameterFloatVector(ros::NodeHandle& nodeHandle, std::string name, std::vector<float>& val, int length)
+// {
+// if(!nodeHandle.getParam(name, val)){
+// ROS_ERROR_STREAM("missing parameter '" << name << "'");
+// }
+// if(val.size() != length) {
+// ROS_ERROR_STREAM("parameter '" << name << "' has wrong array length, " << length << " needed");
+// }
+// }
+// // This function DOES NOT NEED TO BE edited for successful completion of the PPS exercise
+// int getParameterInt(ros::NodeHandle& nodeHandle, std::string name)
+// {
+// int val;
+// if(!nodeHandle.getParam(name, val))
+// {
+// ROS_ERROR_STREAM("missing parameter '" << name << "'");
+// }
+// return val;
+// }
+// // This function DOES NOT NEED TO BE edited for successful completion of the PPS exercise
+// void getParameterIntVectorWithKnownLength(ros::NodeHandle& nodeHandle, std::string name, std::vector<int>& val, int length)
+// {
+// if(!nodeHandle.getParam(name, val)){
+// ROS_ERROR_STREAM("missing parameter '" << name << "'");
+// }
+// if(val.size() != length) {
+// ROS_ERROR_STREAM("parameter '" << name << "' has wrong array length, " << length << " needed");
+// }
+// }
+// // This function DOES NOT NEED TO BE edited for successful completion of the PPS exercise
+// int getParameterIntVectorWithUnknownLength(ros::NodeHandle& nodeHandle, std::string name, std::vector<int>& val)
+// {
+// if(!nodeHandle.getParam(name, val)){
+// ROS_ERROR_STREAM("missing parameter '" << name << "'");
+// }
+// return val.size();
+// }
+// // This function DOES NOT NEED TO BE edited for successful completion of the PPS exercise
+// bool getParameterBool(ros::NodeHandle& nodeHandle, std::string name)
+// {
+// bool val;
+// if(!nodeHandle.getParam(name, val))
+// {
+// ROS_ERROR_STREAM("missing parameter '" << name << "'");
+// }
+// return val;
+// }
@@ -1546,107 +1720,186 @@ int main(int argc, char* argv[]) {
// Starting the ROS-node
ros::init(argc, argv, "PickerControllerService");
- // Create a "ros::NodeHandle" type local variable "nodeHandle" as the current node,
- // the "~" indcates that "self" is the node handle assigned to this variable.
+ // Create a "ros::NodeHandle" type local variable "nodeHandle"
+ // as the current node, the "~" indcates that "self" is the
+ // node handle assigned to this variable.
ros::NodeHandle nodeHandle("~");
// Get the namespace of this "PickerControllerService" node
std::string m_namespace = ros::this_node::getNamespace();
ROS_INFO_STREAM("[PICKER CONTROLLER] ros::this_node::getNamespace() = " << m_namespace);
- // Get the agent ID as the "ROS_NAMESPACE" this computer.
- // NOTES:
- // > If you look at the "Student.launch" file in the "launch" folder, you will see
- // the following line of code:
- // <param name="studentID" value="$(optenv ROS_NAMESPACE)" />
- // This line of code adds a parameter named "studentID" to the "PPSClient"
- // > Thus, to get access to this "studentID" paremeter, we first need to get a handle
- // to the "PPSClient" node within which this controller service is nested.
-
- // Get the handle to the "PPSClient" node
- ros::NodeHandle PPSClient_nodeHandle(m_namespace + "/PPSClient");
- // Get the value of the "studentID" parameter into the instance variable "my_agentID"
- if(!PPSClient_nodeHandle.getParam("agentID", my_agentID))
- {
- // Throw an error if the student ID parameter could not be obtained
- ROS_ERROR("[PICKER CONTROLLER] Failed to get agentID from PPSClient");
+
+
+ // AGENT ID AND COORDINATOR ID
+
+ // NOTES:
+ // > If you look at the "Agent.launch" file in the "launch" folder,
+ // you will see the following line of code:
+ // <param name="agentID" value="$(optenv ROS_NAMESPACE)" />
+ // This line of code adds a parameter named "agentID" to the
+ // "PPSClient" node.
+ // > Thus, to get access to this "studentID" paremeter, we first
+ // need to get a handle to the "PPSClient" node within which this
+ // controller service is nested.
+
+
+ // Get the ID of the agent and its coordinator
+ bool isValid_IDs = getAgentIDandCoordIDfromClientNode( m_namespace + "/PPSClient" , &m_agentID , &m_coordID);
+
+ // Stall the node IDs are not valid
+ if ( !isValid_IDs )
+ {
+ ROS_ERROR("[PICKER CONTROLLER] Node NOT FUNCTIONING :-)");
+ ros::spin();
+ }
+ else
+ {
+ ROS_INFO_STREAM("[PICKER CONTROLLER] loaded agentID = " << m_agentID << ", and coordID = " << m_coordID);
}
+
- // *********************************************************************************
- // EVERYTHING THAT RELATES TO FETCHING PARAMETERS FROM A YAML FILE
+ // PARAMETER SERVICE NAMESPACE AND NODEHANDLES:
+ // NOTES:
+ // > The parameters that are specified thorugh the *.yaml files
+ // are managed by a separate node called the "Parameter Service"
+ // > A separate node is used for reasons of speed and generality
+ // > To allow for a distirbuted architecture, there are two
+ // "ParamterService" nodes that are relevant:
+ // 1) the one that is nested under the "m_agentID" namespace
+ // 2) the one that is nested under the "m_coordID" namespace
+ // > The following lines of code create the namespace (as strings)
+ // to there two relevant "ParameterService" nodes.
+ // > The node handles are also created because they are needed
+ // for the ROS Subscriptions that follow.
- // EVERYTHING FOR THE CONNECTION TO THIS AGENT's OWN PARAMETER SERVICE:
+ // Set the class variable "m_namespace_to_own_agent_parameter_service",
+ // i.e., the namespace of parameter service for this agent
+ m_namespace_to_own_agent_parameter_service = m_namespace + "/ParameterService";
- // Set the class variable "namespace_to_own_agent_parameter_service" to be a the
- // namespace string for the parameter service that is running on the machine of this
- // agent
- namespace_to_own_agent_parameter_service = m_namespace + "/ParameterService";
+ // Set the class variable "m_namespace_to_coordinator_parameter_service",
+ // i.e., the namespace of parameter service for this agent's coordinator
+ constructNamespaceForCoordinatorParameterService( m_coordID, m_namespace_to_coordinator_parameter_service );
- // Create a node handle to the parameter service running on this agent's machine
- ros::NodeHandle nodeHandle_to_own_agent_parameter_service(namespace_to_own_agent_parameter_service);
+ // Inform the user of what namespaces are being used
+ ROS_INFO_STREAM("[PICKER CONTROLLER] m_namespace_to_own_agent_parameter_service = " << m_namespace_to_own_agent_parameter_service);
+ ROS_INFO_STREAM("[PICKER CONTROLLER] m_namespace_to_coordinator_parameter_service = " << m_namespace_to_coordinator_parameter_service);
- // Instantiate the local variable "controllerYamlReadyForFetchSubscriber" to be a
- // "ros::Subscriber" type variable that subscribes to the "controllerYamlReadyForFetch" topic
- // and calls the class function "yamlReadyForFetchCallback" each time a message is
- // received on this topic and the message is passed as an input argument to the
- // "yamlReadyForFetchCallback" class function.
- ros::Subscriber controllerYamlReadyForFetchSubscriber_to_agent = nodeHandle_to_own_agent_parameter_service.subscribe("controllerYamlReadyForFetch", 1, yamlReadyForFetchCallback);
+ // Create, as local variables, node handles to the parameters services
+ ros::NodeHandle nodeHandle_to_own_agent_parameter_service(m_namespace_to_own_agent_parameter_service);
+ ros::NodeHandle nodeHandle_to_coordinator_parameter_service(m_namespace_to_coordinator_parameter_service);
+
+
+
+ // SUBSCRIBE TO "YAML PARAMTERS READY" MESSAGES
+
+ // The parameter service publishes messages with names of the form:
+ // /dfall/.../ParameterService/<filename with .yaml extension>
+ ros::Subscriber pickerContoller_yamlReady_fromAgent = nodeHandle_to_own_agent_parameter_service.subscribe( "PickerController", 1, isReadyStudentControllerYamlCallback);
+ ros::Subscriber pickerContoller_yamlReady_fromCoord = nodeHandle_to_coordinator_parameter_service.subscribe("PickerController", 1, isReadyStudentControllerYamlCallback);
+
+
+
+ // GIVE YAML VARIABLES AN INITIAL VALUE
+ // This can be done either here or as part of declaring the
+ // variables in the header file
+
+
+
+ // FETCH ANY PARAMETERS REQUIRED FROM THE "PARAMETER SERVICES"
+
+ // The yaml files for the controllers are not added to
+ // "Parameter Service" as part of launching.
+ // The process for loading the yaml parameters is to send a
+ // service call containing the filename of the *.yaml file,
+ // and then a message will be received on the above subscribers
+ // when the paramters are ready.
+ // > NOTE IMPORTANTLY that by using a serice client
+ // we stall the availability of this node until the
+ // paramter service is ready
+
+ // Create the service client as a local variable
+ ros::ServiceClient requestLoadYamlFilenameServiceClient = nodeHandle_to_own_agent_parameter_service.serviceClient<LoadYamlFromFilename>("requestLoadYamlFilename", false);
+ // Create the service call as a local variable
+ LoadYamlFromFilename loadYamlFromFilenameCall;
+ // Specify the Yaml filename as a string
+ loadYamlFromFilenameCall.request.stringWithHeader.data = "StudentController";
+ // Set for whom this applies to
+ loadYamlFromFilenameCall.request.stringWithHeader.shouldCheckForID = false;
+ // Wait until the serivce exists
+ requestLoadYamlFilenameServiceClient.waitForExistence(ros::Duration(-1));
+ // Make the service call
+ if(requestLoadYamlFilenameServiceClient.call(loadYamlFromFilenameCall))
+ {
+ // Nothing to do in this case.
+ // The "isReadyStudentControllerYamlCallback" function
+ // will be called once the YAML file is loaded
+ }
+ else
+ {
+ // Inform the user
+ ROS_ERROR("[STUDENT CONTROLLER] The request load yaml file service call failed.");
+ }
- // EVERYTHING FOR THE CONNECTION THE COORDINATOR'S PARAMETER SERVICE:
- // Set the class variable "nodeHandle_to_coordinator_parameter_service" to be a node handle
- // for the parameter service that is running on the coordinate machine
- // NOTE: the backslash here (i.e., "/") before the name of the node ("ParameterService")
- // is very important because it specifies that the name is global
- namespace_to_coordinator_parameter_service = "/ParameterService";
- // Create a node handle to the parameter service running on the coordinator machine
- ros::NodeHandle nodeHandle_to_coordinator = ros::NodeHandle();
- //ros::NodeHandle nodeHandle_to_coordinator_parameter_service = ros::NodeHandle(namespace_to_own_agent_parameter_service);
-
- // Instantiate the local variable "controllerYamlReadyForFetchSubscriber" to be a
- // "ros::Subscriber" type variable that subscribes to the "controllerYamlReadyForFetch" topic
- // and calls the class function "yamlReadyForFetchCallback" each time a message is
- // received on this topic and the message is passed as an input argument to the
- // "yamlReadyForFetchCallback" class function.
- ros::Subscriber controllerYamlReadyForFetchSubscriber_to_coordinator = nodeHandle_to_coordinator.subscribe("/ParameterService/controllerYamlReadyForFetch", 1, yamlReadyForFetchCallback);
- //ros::Subscriber controllerYamlReadyForFetchSubscriber_to_coordinator = nodeHandle_to_coordinator_parameter_service.subscribe("controllerYamlReadyForFetch", 1, yamlReadyForFetchCallback);
+ // INITIALISE OTHER VARIABLES AS REQUIRED
+ m_mass_total_in_grams = yaml_mass_cf_in_grams;
+ m_weight_total_in_newtons = m_mass_total_in_grams * (9.81/1000.0);
- // PRINT OUT SOME INFORMATION
- // Let the user know what namespaces are being used for linking to the parameter service
- ROS_INFO_STREAM("[PICKER CONTROLLER] the namespace strings for accessing the Paramter Services are:");
- ROS_INFO_STREAM("[PICKER CONTROLLER] namespace_to_own_agent_parameter_service = " << namespace_to_own_agent_parameter_service);
- ROS_INFO_STREAM("[PICKER CONTROLLER] namespace_to_coordinator_parameter_service = " << namespace_to_coordinator_parameter_service);
- // FINALLY, FETCH ANY PARAMETERS REQUIRED FROM THESE "PARAMETER SERVICES"
+ // PUBLISHERS AND SUBSCRIBERS
- // Call the class function that loads the parameters for this class.
- fetchYamlParameters(nodeHandle_to_own_agent_parameter_service);
+ // Instantiate the class variable "m_debugPublisher" to be a
+ // "ros::Publisher". This variable advertises under the name
+ // "DebugTopic" and is a message with the structure defined
+ // in the file "DebugMsg.msg" (located in the "msg" folder).
+ m_debugPublisher = nodeHandle.advertise<DebugMsg>("DebugTopic", 1);
- // Initialise the total mass to be just the crazyflie
- m_mass_total_grams = m_mass_CF_grams;
+ // Instantiate the local variable "requestSetpointChangeSubscriber"
+ // to be a "ros::Subscriber" type variable that subscribes to the
+ // "RequestSetpointChange" topic and calls the class function
+ // "requestSetpointChangeCallback" each time a messaged is received
+ // on this topic and the message is passed as an input argument to
+ // the callback function. This subscriber will mainly receive
+ // messages from the "flying agent GUI" when the setpoint is changed
+ // by the user.
+ ros::Subscriber requestSetpointChangeSubscriber = nodeHandle.subscribe("RequestSetpointChange", 1, requestSetpointChangeCallback);
- // *********************************************************************************
+ // Same again but instead for changes requested by the coordinator.
+ // For this we need to first create a node handle to the coordinator:
+ std::string namespace_to_coordinator;
+ constructNamespaceForCoordinator( m_coordID, namespace_to_coordinator );
+ ros::NodeHandle nodeHandle_to_coordinator(namespace_to_coordinator);
+ // And now we can instantiate the subscriber:
+ ros::Subscriber requestSetpointChangeSubscriber_from_coord = nodeHandle_to_coordinator.subscribe("PickerControllerService/RequestSetpointChange", 1, requestSetpointChangeCallback);
+ // Instantiate the class variable "m_setpointChangedPublisher" to
+ // be a "ros::Publisher". This variable advertises under the name
+ // "SetpointChanged" and is a message with the structure defined
+ // in the file "SetpointWithHeader.msg" (located in the "msg" folder).
+ // This publisher is used by the "flying agent GUI" to update the
+ // field that displays the current setpoint for this controller.
+ m_setpointChangedPublisher = nodeHandle.advertise<SetpointWithHeader>("SetpointChanged", 1);
+ // Instantiate the local variable "getCurrentSetpointService" to be
+ // a "ros::ServiceServer" type variable that advertises the service
+ // called "GetCurrentSetpoint". This service has the input-output
+ // behaviour defined in the "GetSetpointService.srv" file (located
+ // in the "srv" folder). This service, when called, is provided with
+ // 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);
- // Instantiate the instance variable "debugPublisher" to be a "ros::Publisher" that
- // advertises under the name "DebugTopic" and is a message with the structure
- // defined in the file "DebugMsg.msg" (located in the "msg" folder).
- debugPublisher = nodeHandle.advertise<DebugMsg>("DebugTopic", 1);
- // Instantiate the local variable "setpointSubscriber" to be a "ros::Subscriber"
- // type variable that subscribes to the "Setpoint" topic and calls the class function
- // "setpointCallback" each time a messaged is received on this topic and the message
- // is passed as an input argument to the "setpointCallback" class function.
- ros::Subscriber setpointSubscriber = nodeHandle.subscribe("Setpoint", 1, setpointCallback);
// Instantiate the local variable "service" to be a "ros::ServiceServer" type
// variable that advertises the service called "PickerController". This service has
@@ -1662,13 +1915,13 @@ int main(int argc, char* argv[]) {
// to the name space of this node, i.e., "d_fall_pps" as specified by the line:
// "using namespace d_fall_pps;"
// in the "DEFINES" section at the top of this file.
- ros::NodeHandle namespace_nodeHandle(ros::this_node::getNamespace());
+ //ros::NodeHandle namespace_nodeHandle(ros::this_node::getNamespace());
// Instantiate the instance variable "my_current_xyz_yaw_publisher" to be a "ros::Publisher"
// that advertises under the name "<my_agentID>/my_current_xyz_yaw_topic" where <my_agentID>
// is filled in with the student ID number of this computer. The messages published will
// have the structure defined in the file "Setpoint.msg" (located in the "msg" folder).
- my_current_xyz_yaw_publisher = nodeHandle.advertise<Setpoint>("my_current_xyz_yaw_topic", 1);
+ //my_current_xyz_yaw_publisher = nodeHandle.advertise<Setpoint>("my_current_xyz_yaw_topic", 1);
// Instantiate the local variable "customCommandSubscriber" to be a "ros::Subscriber"
// type variable that subscribes to the "StudentCustomButton" topic and calls the class
@@ -1678,17 +1931,17 @@ int main(int argc, char* argv[]) {
- // ADDED FOR THE PICKER
- ros::Subscriber pickerButtonPressedSubscriber = nodeHandle.subscribe("ButtonPressed", 1, buttonPressedCallback);
- ros::Subscriber pickerZSetpointSubscriber = nodeHandle.subscribe("ZSetpoint", 1, zSetpointCallback);
- ros::Subscriber pickerYawSetpointSubscriber = nodeHandle.subscribe("YawSetpoint", 1, yawSetpointCallback);
- ros::Subscriber pickerMassSubscriber = nodeHandle.subscribe("Mass", 1, massCallback);
- ros::Subscriber pickerXAdjustmentSubscriber = nodeHandle.subscribe("XAdjustment", 1, xAdjustmentCallback);
- ros::Subscriber pickerYAdjustmentSubscriber = nodeHandle.subscribe("YAdjustment", 1, yAdjustmentCallback);
+ // // ADDED FOR THE PICKER
+ // ros::Subscriber pickerButtonPressedSubscriber = nodeHandle.subscribe("ButtonPressed", 1, buttonPressedCallback);
+ // ros::Subscriber pickerZSetpointSubscriber = nodeHandle.subscribe("ZSetpoint", 1, zSetpointCallback);
+ // ros::Subscriber pickerYawSetpointSubscriber = nodeHandle.subscribe("YawSetpoint", 1, yawSetpointCallback);
+ // ros::Subscriber pickerMassSubscriber = nodeHandle.subscribe("Mass", 1, massCallback);
+ // ros::Subscriber pickerXAdjustmentSubscriber = nodeHandle.subscribe("XAdjustment", 1, xAdjustmentCallback);
+ // ros::Subscriber pickerYAdjustmentSubscriber = nodeHandle.subscribe("YAdjustment", 1, yAdjustmentCallback);
- pickerSetpointToGUIPublisher = nodeHandle.advertise<Setpoint>("SetpointToGUI", 1);
+ // pickerSetpointToGUIPublisher = nodeHandle.advertise<Setpoint>("SetpointToGUI", 1);
- ros::Subscriber pickerButtonPressedWithSetpointSubscriber = nodeHandle.subscribe("ButtonPressedWithSetpoint", 1, buttonPressedWithSetpointCallback);
+ // ros::Subscriber pickerButtonPressedWithSetpointSubscriber = nodeHandle.subscribe("ButtonPressedWithSetpoint", 1, buttonPressedWithSetpointCallback);
diff --git a/pps_ws/src/d_fall_pps/src/nodes/StudentControllerService.cpp b/pps_ws/src/d_fall_pps/src/nodes/StudentControllerService.cpp
index c7e22404..426be3b4 100644
--- a/pps_ws/src/d_fall_pps/src/nodes/StudentControllerService.cpp
+++ b/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);
--
GitLab