diff --git a/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/src/defaultcontrollertab.cpp b/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/src/defaultcontrollertab.cpp
index c4ad82b415137266125767fd59be9d84d7f95645..c527b64103b09f65ccc493098c10de34c8e06e4d 100644
--- a/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/src/defaultcontrollertab.cpp
+++ b/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/src/defaultcontrollertab.cpp
@@ -138,17 +138,18 @@ void DefaultControllerTab::setMeasuredPose(float x , float y , float z , float r
ui->lineEdit_measured_z->setText(qstr + QString::number( z, 'f', 3));
if (roll < 0.0f) qstr = ""; else qstr = "+";
- ui->lineEdit_measured_roll->setText(qstr + QString::number( roll * RAD2DEG, 'f', 1));
+ ui->lineEdit_measured_roll->setText(qstr + QString::number( roll * RAD2DEG, 'f', 1));
if (pitch < 0.0f) qstr = ""; else qstr = "+";
ui->lineEdit_measured_pitch->setText(qstr + QString::number( pitch * RAD2DEG, 'f', 1));
if (yaw < 0.0f) qstr = ""; else qstr = "+";
- ui->lineEdit_measured_yaw->setText(qstr + QString::number( yaw * RAD2DEG, 'f', 1));
+ ui->lineEdit_measured_yaw->setText(qstr + QString::number( yaw * RAD2DEG, 'f', 1));
- // GET THE CURRENT SETPOINT
+ // GET THE CURRENT SETPOINT ERROR
float error_x = x - (ui->lineEdit_setpoint_current_x->text() ).toFloat();
float error_y = y - (ui->lineEdit_setpoint_current_y->text() ).toFloat();
float error_z = z - (ui->lineEdit_setpoint_current_z->text() ).toFloat();
- float error_yaw = yaw - (ui->lineEdit_setpoint_current_yaw->text()).toFloat();
+
+ float error_yaw_deg = yaw * RAD2DEG - (ui->lineEdit_setpoint_current_yaw->text()).toFloat();
// UPDATE THE ERROR COLUMN
if (error_x < 0.0f) qstr = ""; else qstr = "+";
@@ -158,8 +159,8 @@ void DefaultControllerTab::setMeasuredPose(float x , float y , float z , float r
if (error_z < 0.0f) qstr = ""; else qstr = "+";
ui->lineEdit_error_z->setText(qstr + QString::number( error_z, 'f', 3));
- if (error_yaw < 0.0f) qstr = ""; else qstr = "+";
- ui->lineEdit_error_yaw->setText(qstr + QString::number( error_yaw * RAD2DEG, 'f', 1));
+ if (error_yaw_deg < 0.0f) qstr = ""; else qstr = "+";
+ ui->lineEdit_error_yaw->setText(qstr + QString::number( error_yaw_deg , 'f', 1));
// Ensure the red frames are not visible
if ( ui->red_frame_position_left->isVisible() )
diff --git a/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/src/pickercontrollertab.cpp b/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/src/pickercontrollertab.cpp
index 60c5ada32a4118755fe42787feb2ade3ee68a1b3..f90b07f4c9ded2fb6ef36296e73d9baf71489cc1 100644
--- a/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/src/pickercontrollertab.cpp
+++ b/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/src/pickercontrollertab.cpp
@@ -552,13 +552,14 @@ void PickerControllerTab::setMeasuredPose(float x , float y , float z , float ro
ui->lineEdit_measured_z->setText(qstr + QString::number( z, 'f', DECIMAL_PLACES_POSITION));
if (yaw < 0.0f) qstr = ""; else qstr = "+";
- ui->lineEdit_measured_yaw->setText(qstr + QString::number( yaw * RAD2DEG, 'f', DECIMAL_PLACES_ANGLE_DEGREES));
+ ui->lineEdit_measured_yaw->setText(qstr + QString::number( yaw * RAD2DEG, 'f', DECIMAL_PLACES_ANGLE_DEGREES));
- // GET THE CURRENT SETPOINT
+ // GET THE CURRENT SETPOINT ERROR
float error_x = x - (ui->lineEdit_current_x->text() ).toFloat();
float error_y = y - (ui->lineEdit_current_y->text() ).toFloat();
float error_z = z - (ui->lineEdit_current_z->text() ).toFloat();
- float error_yaw = yaw - (ui->lineEdit_current_yaw->text()).toFloat();
+
+ float error_yaw_deg = yaw * RAD2DEG - (ui->lineEdit_current_yaw->text()).toFloat();
// UPDATE THE ERROR COLUMN
if (error_x < 0.0f) qstr = ""; else qstr = "+";
@@ -568,8 +569,8 @@ void PickerControllerTab::setMeasuredPose(float x , float y , float z , float ro
if (error_z < 0.0f) qstr = ""; else qstr = "+";
ui->lineEdit_error_z->setText(qstr + QString::number( error_z, 'f', DECIMAL_PLACES_POSITION));
- if (error_yaw < 0.0f) qstr = ""; else qstr = "+";
- ui->lineEdit_error_yaw->setText(qstr + QString::number( error_yaw * RAD2DEG, 'f', DECIMAL_PLACES_ANGLE_DEGREES));
+ if (error_yaw_deg < 0.0f) qstr = ""; else qstr = "+";
+ ui->lineEdit_error_yaw->setText(qstr + QString::number( error_yaw_deg , 'f', DECIMAL_PLACES_ANGLE_DEGREES));
// Ensure the red frames are not visible
if ( ui->frame_x_unavailable->isVisible() )
diff --git a/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/src/studentcontrollertab.cpp b/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/src/studentcontrollertab.cpp
index e89ddaff7856e7e083ac86ef05eab1ebc56f7649..440daaac14f06788852f06f82dad300ce0294510 100644
--- a/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/src/studentcontrollertab.cpp
+++ b/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/src/studentcontrollertab.cpp
@@ -220,17 +220,18 @@ void StudentControllerTab::setMeasuredPose(float x , float y , float z , float r
ui->lineEdit_measured_z->setText(qstr + QString::number( z, 'f', 3));
if (roll < 0.0f) qstr = ""; else qstr = "+";
- ui->lineEdit_measured_roll->setText(qstr + QString::number( roll * RAD2DEG, 'f', 1));
+ ui->lineEdit_measured_roll->setText(qstr + QString::number( roll * RAD2DEG, 'f', 1));
if (pitch < 0.0f) qstr = ""; else qstr = "+";
ui->lineEdit_measured_pitch->setText(qstr + QString::number( pitch * RAD2DEG, 'f', 1));
if (yaw < 0.0f) qstr = ""; else qstr = "+";
- ui->lineEdit_measured_yaw->setText(qstr + QString::number( yaw * RAD2DEG, 'f', 1));
+ ui->lineEdit_measured_yaw->setText(qstr + QString::number( yaw * RAD2DEG, 'f', 1));
- // GET THE CURRENT SETPOINT
+ // GET THE CURRENT SETPOINT ERROR
float error_x = x - (ui->lineEdit_setpoint_current_x->text() ).toFloat();
float error_y = y - (ui->lineEdit_setpoint_current_y->text() ).toFloat();
float error_z = z - (ui->lineEdit_setpoint_current_z->text() ).toFloat();
- float error_yaw = yaw - (ui->lineEdit_setpoint_current_yaw->text()).toFloat();
+
+ float error_yaw_deg = yaw * RAD2DEG - (ui->lineEdit_setpoint_current_yaw->text()).toFloat();
// UPDATE THE ERROR COLUMN
if (error_x < 0.0f) qstr = ""; else qstr = "+";
@@ -240,8 +241,8 @@ void StudentControllerTab::setMeasuredPose(float x , float y , float z , float r
if (error_z < 0.0f) qstr = ""; else qstr = "+";
ui->lineEdit_error_z->setText(qstr + QString::number( error_z, 'f', 3));
- if (error_yaw < 0.0f) qstr = ""; else qstr = "+";
- ui->lineEdit_error_yaw->setText(qstr + QString::number( error_yaw * RAD2DEG, 'f', 1));
+ if (error_yaw_deg < 0.0f) qstr = ""; else qstr = "+";
+ ui->lineEdit_error_yaw->setText(qstr + QString::number( error_yaw_deg , 'f', 1));
// Ensure the red frames are not visible
if ( ui->red_frame_position_left->isVisible() )
diff --git a/dfall_ws/src/dfall_pkg/include/nodes/PickerControllerService.h b/dfall_ws/src/dfall_pkg/include/nodes/PickerControllerService.h
index fab684a889280fee19ed64ba2ae56f7660c73df3..f0d015afeed077db8ff354e12cd72f533ee9642e 100644
--- a/dfall_ws/src/dfall_pkg/include/nodes/PickerControllerService.h
+++ b/dfall_ws/src/dfall_pkg/include/nodes/PickerControllerService.h
@@ -154,7 +154,7 @@ float m_weight_total_in_newtons;
// The setpoint to be tracked, the ordering is (x,y,z,yaw),
// with units [meters,meters,meters,radians]
-float m_setpoint[4] = {0.0,0.0,0.4,0.0};
+float m_setpoint[4] = {0.0,0.0,0.4,0.0};
// The setpoint that is actually used by the controller, this
// differs from the setpoint when smoothing is turned on
@@ -164,10 +164,10 @@ float m_setpoint_for_controller[4] = {0.0,0.0,0.4,0.0};
bool m_shouldSmoothSetpointChanges = true;
// Max setpoint change per second
-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;
+float yaml_max_setpoint_change_per_second_horizontal = 0.1;
+float yaml_max_setpoint_change_per_second_vertical = 0.1;
+float yaml_max_setpoint_change_per_second_yaw_degrees = 90.0;
+float m_max_setpoint_change_per_second_yaw_radians = 90.0 * DEG2RAD;
@@ -267,7 +267,7 @@ 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 yaml_shouldPerformConvertIntoBodyFrame = false;
+bool yaml_shouldPerformConvertIntoBodyFrame = true;
// VARIABLES RELATING TO THE PUBLISHING OF A DEBUG MESSAGE
diff --git a/dfall_ws/src/dfall_pkg/param/PickerController.yaml b/dfall_ws/src/dfall_pkg/param/PickerController.yaml
index f319b4fdb9ba6e4d4bf9508eb91cf062d2ff9adc..eb097bb6343e756bfd884e1dc70590f5fc108108 100644
--- a/dfall_ws/src/dfall_pkg/param/PickerController.yaml
+++ b/dfall_ws/src/dfall_pkg/param/PickerController.yaml
@@ -2,7 +2,7 @@
# 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]
+max_setpoint_change_per_second_yaw_degrees : 180.00 # [degrees]
diff --git a/dfall_ws/src/dfall_pkg/src/nodes/PickerControllerService.cpp b/dfall_ws/src/dfall_pkg/src/nodes/PickerControllerService.cpp
index deda6de0cb2c53ca33b0c08d11017fdd8d76fb40..6d28bc65675b30c43d80ab94240d15cbb646d92a 100644
--- a/dfall_ws/src/dfall_pkg/src/nodes/PickerControllerService.cpp
+++ b/dfall_ws/src/dfall_pkg/src/nodes/PickerControllerService.cpp
@@ -815,18 +815,20 @@ void convert_stateInertial_to_bodyFrameError(float stateInertial[12], float setp
// > Second, "unwrap" the yaw error to the interval ( -pi , pi )
while(yawError > PI) {yawError -= 2 * PI;}
while(yawError < -PI) {yawError += 2 * PI;}
- // > Third, put the "yawError" into the "stateError" variable
- stateInertial[8] = yawError;
-
-
- if (yawError>(PI/6))
+ // > Third, clip the error to within some bounds
+ if (yawError>(PI/3))
{
- yawError = (PI/6);
+ yawError = (PI/3);
}
- else if (yawError<(-PI/6))
+ else if (yawError<(-PI/3))
{
- yawError = (-PI/6);
+ yawError = (-PI/3);
}
+ // > Finally, put the "yawError" into the "stateError" variable
+ stateInertial[8] = yawError;
+
+
+
// CONVERSION INTO BODY FRAME
// Conver the state erorr from the Inertial frame into the Body frame
@@ -1056,7 +1058,6 @@ bool getCurrentSetpointCallback(GetSetpointService::Request &request, GetSetpoin
// PUBLISH THE CURRENT X,Y,Z, AND YAW (if required)
void publish_current_xyz_yaw(float x, float y, float z, float yaw)
{
- // // publish setpoint for FollowController of another student group
// SetpointWithHeader temp_current_xyz_yaw;
// // Fill in the x,y,z, and yaw info directly from the data provided to this
// // function
@@ -1135,7 +1136,7 @@ void isReadyPickerControllerYamlCallback(const IntWithHeader & msg)
void fetchPickerControllerYamlParameters(ros::NodeHandle& nodeHandle)
{
// Here we load the parameters that are specified in the file:
- // StudentController.yaml
+ // PickerController.yaml
// Add the "PickerController" namespace to the "nodeHandle"
ros::NodeHandle nodeHandle_for_paramaters(nodeHandle, "PickerController");
@@ -1203,7 +1204,7 @@ void fetchPickerControllerYamlParameters(ros::NodeHandle& nodeHandle)
// DEBUGGING: Print out one of the parameters that was loaded
- ROS_INFO_STREAM("[PICKER CONTROLLER] DEBUGGING: the fetched PickerController/mass_cf_in_grams = " << yaml_mass_cf_in_grams);
+ ROS_INFO_STREAM("[PICKER CONTROLLER] DEBUGGING: the fetched PickerController/max_setpoint_change_per_second_yaw_degrees = " << yaml_max_setpoint_change_per_second_yaw_degrees);
@@ -1214,6 +1215,10 @@ void fetchPickerControllerYamlParameters(ros::NodeHandle& nodeHandle)
// Set that the estimator frequency is the same as the control frequency
m_estimator_frequency = yaml_control_frequency;
+ // DEBUGGING: Print out one of the processed values
+ ROS_INFO_STREAM("[PICKER CONTROLLER] DEBUGGING: after processing m_max_setpoint_change_per_second_yaw_radians = " << m_max_setpoint_change_per_second_yaw_radians);
+
+
}
@@ -1256,7 +1261,7 @@ int main(int argc, char* argv[]) {
// <param name="agentID" value="$(optenv ROS_NAMESPACE)" />
// This line of code adds a parameter named "agentID" to the
// "FlyingAgentClient" node.
- // > Thus, to get access to this "studentID" paremeter, we first
+ // > Thus, to get access to this "agentID" paremeter, we first
// need to get a handle to the "FlyingAgentClient" node within which this
// controller service is nested.
@@ -1351,13 +1356,13 @@ int main(int argc, char* argv[]) {
if(requestLoadYamlFilenameServiceClient.call(loadYamlFromFilenameCall))
{
// Nothing to do in this case.
- // The "isReadyStudentControllerYamlCallback" function
+ // The "isReadyPickerControllerYamlCallback" 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.");
+ ROS_ERROR("[PICKER CONTROLLER] The request load yaml file service call failed.");
}
@@ -1430,13 +1435,13 @@ int main(int argc, char* argv[]) {
// 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
+ // is filled in with the agent 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);
// Print out some information to the user.
- ROS_INFO("[Picker CONTROLLER] Service ready :-)");
+ ROS_INFO("[PICKER CONTROLLER] Service ready :-)");
// Enter an endless while loop to keep the node alive.
ros::spin();