Commit ac1cb5ab authored by beuchatp's avatar beuchatp
Browse files

Small changes to the picker controller for paramter loading, and to Flying...

Small changes to the picker controller for paramter loading, and to Flying agent GUI for units of the yaw error
parent dacae599
......@@ -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() )
......
......@@ -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() )
......
......@@ -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() )
......
......@@ -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
......
......@@ -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]
......
......@@ -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();
......
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment