BOOLEAN
isAngleReachable(Point rpos, float rrot,float *leftAngle,float *rightAngle)
{
float targetDist,alpha1,beta1;
(*leftAngle) = (collisionDistance(rpos,
rrot,
0.0,
-20.0,
&targetDist) * DEG_360) / MAX_RANGE;
(*rightAngle) = (collisionDistance(rpos,
rrot,
0.0,
20.0,
&targetDist) * DEG_360) / MAX_RANGE;
alpha1 = rrot + (*leftAngle);
norm_angle(&alpha1);
beta1 = rrot - (*rightAngle);
norm_angle(&beta1);
if (rightRot)
{
if (rrot>=beta1)
{
if ((beta1<=absoluteAngle) && (absoluteAngle<=rrot))
return TRUE;
else
return FALSE;
}
else
{
if ((rrot>=absoluteAngle) || (absoluteAngle>=beta1))
return TRUE;
else
return FALSE;
}
}
else
{
if (alpha1>=rrot)
{
if ((alpha1>=absoluteAngle) && (absoluteAngle>=rrot))
return TRUE;
else
return FALSE;
}
else
{
if ((rrot<=absoluteAngle) || (absoluteAngle<=alpha1))
return TRUE;
else
return FALSE;
}
}
}
void NavPoint::getArcParams(NavPoint &np, double& L, double& r_c, double& phi_g) {
double x_0=x, y_0=y, z_0=z, th_0=norm_angle(th);
double x_1=np.x, y_1=np.y, z_1=np.z;//, th_1=norm_angle(np.th);
double dx = x_1-x_0;
double dy = y_1-y_0;
double temp_dx = dx;
double th_0_inv = norm_angle(2*M_PI - th_0);
//double th_0_inv = norm_angle(th_0);
dx = dx * cos(th_0_inv) - dy * sin(th_0_inv);
dy = temp_dx * sin(th_0_inv) + dy * cos(th_0_inv);
//double dz = z_1-z_0;
// Compute polar coordinates towards p_1
bool leftTurn = false;
bool rightTurn = false;
bool straight = false;
double r_g = sqrt(dx*dx + dy*dy);
if (dx==0.0) dx += EPS_VAL;
phi_g = norm_angle(atan2(dy,dx));
// Compute desired radius (positive means left turn) and length
L = 0.0;
r_c = 0.0;
if (fabs( sin(phi_g) ) < ANGLE_EPSILON ) {
straight = true;
leftTurn=false;
rightTurn=false;
r_c=0.0;
// Compute length of the straight
L = r_g;
}
else {
// Compute the radius
r_c = r_g / (2.0 * sin(phi_g));
// Compute length of the arc
L = (r_g * phi_g) / (sin(phi_g));
if (r_c < 0.0) {
r_c *= -1.0;
straight=false;
leftTurn=true;
rightTurn=false;
}
else {
straight=false;
leftTurn=false;
rightTurn=true;
}
}
}
void StepGuider::SetCalibrationDetails(const CalibrationDetails& calDetails, double xAngle, double yAngle)
{
m_calibrationDetails = calDetails;
m_calibrationDetails.raGuideSpeed = -1.0;
m_calibrationDetails.decGuideSpeed = -1.0;
m_calibrationDetails.focalLength = pFrame->GetFocalLength();
m_calibrationDetails.imageScale = pFrame->GetCameraPixelScale();
m_calibrationDetails.orthoError = degrees(fabs(fabs(norm_angle(xAngle - yAngle)) - M_PI / 2.)); // Delta from the nearest multiple of 90 degrees
m_calibrationDetails.raStepCount = m_calibrationDetails.raSteps.size();
m_calibrationDetails.decStepCount = m_calibrationDetails.decSteps.size();
Mount::SetCalibrationDetails(m_calibrationDetails, xAngle, yAngle);
}
/**********************************************************************
* Given the position of the robot and the distance of a particular
* sonar the collision line representing this reading is given back.
**********************************************************************/
LineSeg sonar_to_lineseg( Point rpos, float rrot,
int sonar_no, float dist)
{
Point sonar_pos, sonar_left_edge, sonar_right_edge;
LineSeg line;
float sonar_rot, sonar_plain, open_angle, tmp, start_angle;
open_angle = SONAR_OPEN_ANGLE;
/* If the line is too far away we don't consider it. */
if (dist > 0.0 && dist <= MAX_RANGE) {
if ( armState == INSIDE) {
/* If the robot is very fast we don't consider the lines behind it.
* If the current tvel is 100.0 we only use lines starting at 90.0
* degrees. This is only allowed if the arm is inside.
*/
tmp = (float) rwi_base.trans_current_speed * 0.01;
start_angle = DEG_180 - tmp * DEG_90;
if ( SonarAngle[sonar_no] > start_angle &&
SonarAngle[sonar_no] < DEG_360 - start_angle) {
line.pt1.x = F_ERROR;
return(line);
}
}
/* We want to limit the length of the lineseg to MAX_COLLISION_LINE_LENGTH */
if (dist > EPSILON && ACTUAL_MODE->max_collision_line_length > 0.0)
if ((tmp = 0.5 * atan(ACTUAL_MODE->max_collision_line_length / dist))
< open_angle)
open_angle = tmp;
sonar_rot = rrot + SonarAngle[sonar_no];
norm_angle(&sonar_rot);
dist /= fcos(open_angle);
sonar_plain = sonar_rot + DEG_90;
sonar_pos.x = rpos.x + (fcos( sonar_rot) * ROB_RADIUS);
sonar_pos.y = rpos.y + (fsin( sonar_rot) * ROB_RADIUS);
sonar_left_edge.x = sonar_pos.x + (fcos( sonar_plain) * COLLI_SONAR_RADIUS);
sonar_left_edge.y = sonar_pos.y + (fsin( sonar_plain) * COLLI_SONAR_RADIUS);
sonar_right_edge.x = sonar_pos.x - (fcos( sonar_plain) * COLLI_SONAR_RADIUS);
sonar_right_edge.y = sonar_pos.y - (fsin( sonar_plain) * COLLI_SONAR_RADIUS);
line.pt1.x = sonar_left_edge.x + fcos( sonar_rot + open_angle) * dist;
line.pt1.y = sonar_left_edge.y + fsin( sonar_rot + open_angle) * dist;
line.pt2.x = sonar_right_edge.x + fcos( sonar_rot - open_angle) * dist;
line.pt2.y = sonar_right_edge.y + fsin( sonar_rot - open_angle) * dist;
/* We want the point with the smaller x value (if possible) to be pt1 */
if (smaller( line.pt2, line.pt1))
swap(&line);
return(line);
}
else line.pt1.x = F_ERROR;
return(line);
}
/* Test-Functions to check the rotate-away functions */
void
printCurrentAngles(Point rpos, float rrot)
{
#define MIN_ROT 5.0
int i;
float targetAngle, targetDist;
float leftDist, rightDist, securityDist;
float leftAngle,rightAngle,alpha1,beta1,leftDistAngle,rightDistAngle;
float angle;
float delta1,delta2;
float resultAngle;
BOOLEAN foundAdmissibleAngle = FALSE;
leftAngle = (collisionDistance(rpos,
rrot,
0.0,
-20.0,
&targetDist) * DEG_360) / MAX_RANGE;
rightAngle = (collisionDistance(rpos,
rrot,
0.0,
20.0,
&targetDist) * DEG_360) / MAX_RANGE;
/* Rotation is not possible */
if((leftAngle <= DEG_TO_RAD(MIN_ROT)) && (rightAngle <= DEG_TO_RAD(MIN_ROT)))
{
printf("RP2: Really, no rotation possible !\n");
rotPossibleFlag = FALSE;
return;
}
printf("leftAngle = %f, rightAngle = %f, rrot = %f \n",RAD_TO_DEG(leftAngle)
,RAD_TO_DEG(rightAngle),RAD_TO_DEG(rrot));
securityDist = 2.0 * ACTUAL_MODE->min_dist;
angle = targetAngle = 0.0;
if ((armState == INSIDE) || ((leftAngle+rightAngle) >= DEG_360))
{
for (i=0; i<=180; i+=10) {
leftDist = computeCollisionDistance(
rpos,
targetAngle + DEG_TO_RAD((float)i),
ACTUAL_MODE->target_max_trans_speed,
0.0,
ROB_RADIUS+ACTUAL_MODE->security_dist,
0.0,0.0,
&targetDist);
rightDist = computeCollisionDistance(
rpos,
targetAngle - DEG_TO_RAD((float)i),
ACTUAL_MODE->target_max_trans_speed,
0.0,
ROB_RADIUS+ACTUAL_MODE->security_dist,
0.0,0.0,
&targetDist);
if (leftDist > rightDist)
{
if (leftDist > securityDist)
{
foundAdmissibleAngle = TRUE;
angle = normed_angle(targetAngle + DEG_TO_RAD((float) i));
}
}
else
{
if (rightDist > securityDist)
{
foundAdmissibleAngle = TRUE;
angle = normed_angle(targetAngle - DEG_TO_RAD((float) i));
}
}
if (foundAdmissibleAngle)
break;
}
if (!foundAdmissibleAngle)
{
printf("RP2 : No angle is admissible !\n");
rotPossibleFlag = FALSE;
return;
}
}
else /* The arm is outside */
{
alpha1 = rrot + leftAngle;
norm_angle(&alpha1);
beta1 = rrot - rightAngle;
norm_angle(&beta1);
for (i=0; i<=180; i+=10) {
leftDistAngle = normed_angle(targetAngle - DEG_TO_RAD((float)i));
rightDistAngle = normed_angle(targetAngle + DEG_TO_RAD((float)i));
leftDist = computeCollisionDistance(
rpos,
targetAngle - DEG_TO_RAD((float)i),
ACTUAL_MODE->target_max_trans_speed,
0.0,
ROB_RADIUS+ACTUAL_MODE->security_dist,
0.0,0.0,
&targetDist);
rightDist = computeCollisionDistance(
rpos,
targetAngle + DEG_TO_RAD((float)i),
ACTUAL_MODE->target_max_trans_speed,
0.0,
ROB_RADIUS+ACTUAL_MODE->security_dist,
0.0,0.0,
&targetDist);
if (alpha1>=beta1)
{
if (leftDist >= rightDist)
{ /* We prefer leftDistAngle */
if ((leftDist> securityDist) &&
((alpha1>=leftDistAngle) && (leftDistAngle>= beta1)))
{
foundAdmissibleAngle = TRUE;
angle = leftDistAngle;
}
else
{
if ((rightDist > securityDist) &&
((alpha1>=rightDistAngle) && (rightDistAngle>=beta1)))
{
foundAdmissibleAngle = TRUE;
angle = rightDistAngle;
}
}
}
else
{ /* We prefer rightDistAngle */
if ((rightDist > securityDist) &&
((alpha1>=rightDistAngle) && (rightDistAngle>= beta1)))
{
foundAdmissibleAngle = TRUE;
angle = rightDistAngle;
}
else
{
if ((leftDist > securityDist) &&
((alpha1>=leftDistAngle) && (leftDistAngle>=beta1)))
{
foundAdmissibleAngle = TRUE;
angle = leftDistAngle;
}
}
}
}
else /* alpha1 < beta1 */
{
if (leftDist >= rightDist)
{ /* We prefer leftDistAngle */
if ((leftDist> securityDist) &&
((alpha1>=leftDistAngle) || (leftDistAngle>= beta1)))
{
foundAdmissibleAngle = TRUE;
angle = leftDistAngle;
}
else
{
if ((rightDist > securityDist) &&
((alpha1>=rightDistAngle) || (rightDistAngle>=beta1)))
{
foundAdmissibleAngle = TRUE;
angle = rightDistAngle;
}
}
}
else
{ /* We prefer rightDistAngle */
if ((rightDist > securityDist) &&
((alpha1>=rightDistAngle) || (rightDistAngle>= beta1)))
{
foundAdmissibleAngle = TRUE;
angle = rightDistAngle;
}
else
{
if ((leftDist > securityDist) &&
((alpha1>=leftDistAngle) || (leftDistAngle>=beta1)))
{
foundAdmissibleAngle = TRUE;
angle = leftDistAngle;
}
}
}
}
if (foundAdmissibleAngle)
break;
}
if(! foundAdmissibleAngle )
{
printf("RP : Can not rotate away because maxDistance<securityDist \n");
rotPossibleFlag = FALSE;
return;
}
}
/* here we know that we are able to turn to the chosen angle "angle" */
delta1 = rrot - angle;
if (delta1<= 0.0)
delta2 = delta1 + DEG_360;
else
delta2 = delta1 - DEG_360;
if (fabs(delta1)<= fabs(delta2))
{ /* We prefer delta1 as rotation angle because it is the smaller one */
if (delta1 >= 0.0)
{ /* We prefer turning right */
if (delta1 <= rightAngle )
{ /* It is possible to turn right */
resultAngle = delta1;
}
else
{ /* It is not possible to turn right, therefore we turn left */
resultAngle = delta2;
}
}
else
{ /* We prefer turning left */
if (fabs(delta1) <= leftAngle)
{ /* It is possible to turn left */
resultAngle = delta1;
}
else
{ /* It is not possible to turn left, therefore we turn right */
resultAngle = delta2;
}
}
}
else
{ /* We prefer delta2 as rotation angle because it is the smaller one */
if (delta2 >= 0.0)
{ /* We prefer turning right */
if (delta2 <= rightAngle )
{ /* It is possible to turn right */
resultAngle = delta2;
}
else
{ /* It is not possible to turn right, therefore we turn left */
resultAngle = delta1;
}
}
else
{ /* We prefer turning left */
if (fabs(delta2) <= leftAngle)
{ /* It is possible to turn left */
resultAngle = delta2;
}
else
{ /* It is not possible to turn left, therefore we turn right */
resultAngle = delta1;
}
}
}
printf("Suggested rotation = %f \n",RAD_TO_DEG(resultAngle));
rotPossibleFlag = FALSE;
return;
}
// Base class version builds data grids showing last calibration details and calibration "context"
void CalReviewDialog::CreateDataGrids(wxPanel* parentPanel, wxSizer* parentHSizer, bool AO)
{
const double siderealSecondPerSec = 0.9973;
const double siderealRate = 15.0 * siderealSecondPerSec;
Calibration calBaseline;
CalibrationDetails calDetails;
if (!pSecondaryMount)
{
pMount->GetCalibrationDetails(&calDetails); // Normal case, no AO
pMount->GetLastCalibration(&calBaseline);
}
else
{
if (AO)
{
pMount->GetCalibrationDetails(&calDetails); // AO tab, use AO details
pMount->GetLastCalibration(&calBaseline);
}
else
{
pSecondaryMount->GetCalibrationDetails(&calDetails); // Mount tab, use mount details
pSecondaryMount->GetLastCalibration(&calBaseline);
}
}
wxBoxSizer* panelGridVSizer = new wxBoxSizer(wxVERTICAL);
parentHSizer->Add(panelGridVSizer, 0, wxALIGN_CENTER_HORIZONTAL | wxALL, 5);
int row = 0;
int col = 0;
bool validDetails = calDetails.raStepCount > 0; // true for non-AO with pointing source info and "recent" calibration
bool validBaselineDeclination = calBaseline.declination != UNKNOWN_DECLINATION;
// Build the upper frame and grid for data from the last calibration
wxStaticBox* staticBoxLastCal = new wxStaticBox(parentPanel, wxID_ANY, _("Last Mount Calibration"));
if (AO)
staticBoxLastCal->SetLabelText(_("Last AO Calibration"));
wxStaticBoxSizer* calibFrame = new wxStaticBoxSizer(staticBoxLastCal, wxVERTICAL | wxEXPAND);
panelGridVSizer->Add(calibFrame, 0, wxALIGN_LEFT | wxALL, 5);
wxGrid* calGrid = new wxGrid(parentPanel, wxID_ANY, wxDefaultPosition, wxDefaultSize, wxSUNKEN_BORDER | wxHSCROLL | wxVSCROLL);
calGrid->SetColLabelSize(0);
calGrid->SetRowLabelSize(0);
calGrid->CreateGrid(5, 4);
calGrid->EnableEditing(false);
calGrid->SetCellValue(row, col++, _("RA steps:"));
if (validDetails)
calGrid->SetCellValue(row, col++, wxString::Format("%d", calDetails.raStepCount));
else
calGrid->SetCellValue(row, col++, NA_STR);
calGrid->SetCellValue(row, col++, _("Dec steps:"));
if (validDetails)
calGrid->SetCellValue(row, col++, wxString::Format("%d", calDetails.decStepCount));
else
calGrid->SetCellValue(row, col++, NA_STR);
row++;
col = 0;
calGrid->SetCellValue(row, col++, _("Camera angle:"));
double cam_angle = degrees(norm_angle(calBaseline.xAngle));
calGrid->SetCellValue(row, col++, wxString::Format("%.1f", cam_angle));
calGrid->SetCellValue(row, col++, _("Orthogonality error:"));
if (validDetails)
calGrid->SetCellValue(row, col++, wxString::Format("%0.1f", calDetails.orthoError));
else
calGrid->SetCellValue(row, col++, NA_STR);
row++;
col = 0;
double guideRaSiderealX = -1.0;
double guideDecSiderealX = -1.0;
if (validDetails && calDetails.raGuideSpeed > 0.0)
{
guideRaSiderealX = calDetails.raGuideSpeed * 3600.0 / siderealRate; // Degrees/sec to Degrees/hour, 15 degrees/hour is roughly sidereal rate
guideDecSiderealX = calDetails.decGuideSpeed * 3600.0 / siderealRate; // Degrees/sec to Degrees/hour, 15 degrees/hour is roughly sidereal rate
}
wxString ARCSECPERSEC(_("a-s/sec"));
wxString PXPERSEC(_("px/sec"));
wxString ARCSECPERPX(_("a-s/px"));
if (!AO)
{
calGrid->SetCellValue(row, col++, _("RA rate:"));
}
else
calGrid->SetCellValue(row, col++, _("X rate:"));
if (validDetails)
calGrid->SetCellValue(row, col++, wxString::Format("%0.3f %s\n%0.3f %s", calBaseline.xRate * 1000 * calDetails.imageScale * calBaseline.binning, ARCSECPERSEC,
calBaseline.xRate * 1000, PXPERSEC));
else
calGrid->SetCellValue(row, col++, wxString::Format("%0.3f %s", calBaseline.xRate * 1000, PXPERSEC)); // just px/sec with no image scale data
if (!AO)
calGrid->SetCellValue(row, col++, _("Dec rate:"));
else
calGrid->SetCellValue(row, col++, _("Y rate:"));
if (calBaseline.yRate != CALIBRATION_RATE_UNCALIBRATED)
{
if (validDetails)
calGrid->SetCellValue(row, col++, wxString::Format("%0.3f %s\n%0.3f %s", calBaseline.yRate * 1000 * calDetails.imageScale * calBaseline.binning, ARCSECPERSEC,
calBaseline.yRate * 1000, PXPERSEC));
else
calGrid->SetCellValue(row, col++, wxString::Format("%0.3f %s", calBaseline.yRate * 1000, PXPERSEC)); // just px/sec with no image scale data
}
else
calGrid->SetCellValue(row, col++, NA_STR);
row++;
col = 0;
if (validDetails && calBaseline.yRate > 0)
{
calGrid->SetCellValue(row, col++, _("Expected RA rate:"));
if (validBaselineDeclination && guideRaSiderealX != -1.0 && fabs(degrees(calBaseline.declination)) < 65.0)
{
// Dec speed setting corrected for pointing position and then for any difference in RA guide speed setting
double expectedRaRate = siderealRate * cos(calBaseline.declination) * guideRaSiderealX;
calGrid->SetCellValue(row, col++, wxString::Format("%0.1f %s", expectedRaRate, ARCSECPERSEC));
}
else
calGrid->SetCellValue(row, col++, NA_STR);
calGrid->SetCellValue(row, col++, _("Expected Dec rate:"));
if (guideRaSiderealX != -1.0)
{
double expectedDecRate = siderealRate * guideDecSiderealX;
calGrid->SetCellValue(row, col++, wxString::Format("%0.1f %s", expectedDecRate, ARCSECPERSEC));
}
else
calGrid->SetCellValue(row, col++, NA_STR);
}
row++;
col = 0;
calGrid->SetCellValue(row, col++, _("Binning:"));
calGrid->SetCellValue(row, col++, wxString::Format("%d", (int)calBaseline.binning));
calGrid->SetCellValue(row, col++, _("Created:"));
calGrid->SetCellValue(row, col++, validDetails ? calDetails.origTimestamp : _("Unknown"));
calGrid->AutoSize();
calibFrame->Add(calGrid, 0, wxALIGN_CENTER_HORIZONTAL | wxALL, 5);
if (!AO) // Don't put this mount-related data on the AO panel
{
// Build the upper frame and grid for configuration data
wxStaticBox* staticBoxMount = new wxStaticBox(parentPanel, wxID_ANY, _("Mount Configuration"));
wxStaticBoxSizer* configFrame = new wxStaticBoxSizer(staticBoxMount, wxVERTICAL);
panelGridVSizer->Add(configFrame, 0, wxALIGN_LEFT | wxALL, 5);
wxGrid* cfgGrid = new wxGrid(parentPanel, wxID_ANY, wxDefaultPosition, wxDefaultSize, wxSUNKEN_BORDER | wxHSCROLL | wxVSCROLL);
row = 0;
col = 0;
cfgGrid->SetColLabelSize(0);
cfgGrid->SetRowLabelSize(0);
cfgGrid->CreateGrid(4, 4);
cfgGrid->EnableEditing(false);
cfgGrid->SetCellValue(row, col++, _("Modified:"));
cfgGrid->SetCellValue(row, col++, calBaseline.timestamp);
cfgGrid->SetCellValue(row, col++, _("Focal length:"));
if (validDetails)
cfgGrid->SetCellValue(row, col++, wxString::Format(_("%d mm"), calDetails.focalLength));
else
cfgGrid->SetCellValue(row, col++, NA_STR);
row++;
col = 0;
cfgGrid->SetCellValue(row, col++, _("Image scale:"));
if (validDetails)
{
wxString binning = wxString::Format(_("Binning: %d"), (int)calDetails.origBinning); // Always binning used in actual calibration
cfgGrid->SetCellValue(row, col++, wxString::Format("%0.2f %s\n%s", calDetails.imageScale, ARCSECPERPX, binning));
}
else
cfgGrid->SetCellValue(row, col++, NA_STR);
cfgGrid->SetCellValue(row, col++, _("Side-of-pier:"));
wxString sPierSide = calBaseline.pierSide == PIER_SIDE_EAST ? _("East") :
calBaseline.pierSide == PIER_SIDE_WEST ? _("West") : NA_STR;
cfgGrid->SetCellValue(row, col++, _(sPierSide));
row++;
col = 0;
cfgGrid->SetCellValue(row, col++, _("RA Guide speed:"));
if (guideRaSiderealX != -1.0) // Do the RA guide setting
{
cfgGrid->SetCellValue(row, col++, wxString::Format(_("%0.2fx"), guideRaSiderealX));
}
else
cfgGrid->SetCellValue(row, col++, NA_STR);
cfgGrid->SetCellValue(row, col++, _("Dec Guide speed:"));
if (guideDecSiderealX != -1.0) // Do the Dec guide setting
{
cfgGrid->SetCellValue(row, col++, wxString::Format(_("%0.2fx"), guideDecSiderealX));
}
else
cfgGrid->SetCellValue(row, col++, NA_STR);
row++;
col = 0;
// dec may be gotten from mount or imputed
double dec = calBaseline.declination;
bool decEstimated = false;
if (!validBaselineDeclination)
{
if (fabs(calBaseline.yRate) > 0.00001 && fabs(calBaseline.xRate / calBaseline.yRate) <= 1.0)
{
dec = acos(calBaseline.xRate / calBaseline.yRate); // RA_Rate = Dec_Rate * cos(dec)
decEstimated = true;
}
}
cfgGrid->SetCellValue(row, col++, _("Declination"));
wxString decStr = Mount::DeclinationStr(dec, "%0.1f");
if (decEstimated)
decStr += _(" (est)");
cfgGrid->SetCellValue(row, col++, decStr);
cfgGrid->SetCellValue(row, col++, _("Rotator position:"));
bool valid_rotator = fabs(calBaseline.rotatorAngle) < 360.0;
if (valid_rotator)
cfgGrid->SetCellValue(row, col++, wxString::Format("%0.1f", calBaseline.rotatorAngle));
else
cfgGrid->SetCellValue(row, col++, NA_STR);
cfgGrid->AutoSize();
configFrame->Add(cfgGrid, 0, wxALIGN_CENTER_HORIZONTAL | wxALL, 5);
}
}
double NavPoint::getCostToPoint(NavPoint &np)
{
double x_0=x, y_0=y, z_0=z, th_0=norm_angle(th);
double x_1=np.x, y_1=np.y, z_1=np.z;//, th_1=norm_angle(np.th);
double dx = x_1-x_0;
double dy = y_1-y_0;
// rotate to make relative to the Navpoints coordinate system
if (DEBUG_NAV)
printf("prior rot dx=%1.2lf, dy=%1.2lf, th=%1.2lf\n",dx,dy,th_0);
double temp_dx = dx;
double th_0_inv = norm_angle(2*M_PI - th_0);
dx = dx * cos(th_0_inv) - dy * sin(th_0_inv);
dy = temp_dx * sin(th_0_inv) + dy * cos(th_0_inv);
double dz = z_1-z_0;
if (DEBUG_NAV)
printf("x=%1.2lf, y=%1.2lf, x1=%1.2lf, y1=%1.2lf\n",x_0,y_0,x_1,y_1);
if (DEBUG_NAV)
printf("local coor dx=%1.2lf, dy=%1.2lf, dz=%1.2lf, th=%1.2lf\n",dx,dy,dz,th_0);
// Compute polar coordinates towards p_1
bool leftTurn = false;
bool rightTurn = false;
bool straight = false;
double r_g = sqrt(dx*dx + dy*dy);
if (dx==0.0) dx += EPS_VAL;
double phi_g = norm_angle(atan2(dy,dx));
if (DEBUG_NAV)
printf("phi_g=%1.2lf\n",phi_g);
// Compute desired radius (positive means left turn) and length
double L = 0.0;
double r_c = 0.0;
if (fabs( sin(phi_g) ) < ANGLE_EPSILON ) {
straight = true;
leftTurn=false;
rightTurn=false;
r_c=0.0;
// Compute length of the straight
L = r_g;
}
else {
// Compute the radius
r_c = r_g / (2.0 * sin(phi_g));
// Compute length of the arc
L = (r_g * phi_g) / (sin(phi_g));
if (r_c < 0.0) {
r_c *= -1.0;
straight=false;
leftTurn=true;
rightTurn=false;
}
else {
straight=false;
leftTurn=false;
rightTurn=true;
}
}
if (DEBUG_NAV)
printf("rad=%1.2lf, turnLeft=%d straight=%d\n",r_c,leftTurn,straight);
// Compute time needed for travelling the arc
if (tv==0.0) tv += EPS_VAL;
// double dt = L / tv;
// Approximation to add influence of altitude change:
double dt = sqrt(dz*dz + L*L) / tv;
// Compute the rotational velocity needed
double theta = norm_angle(2.0 * phi_g);
np.th = np.th + theta;
np.th = norm_angle(np.th);
double omega = theta / dt;
if (!leftTurn) omega *= -1.0;
if (DEBUG_NAV)
printf("L=%1.2lf dt=%1.2lf omega=%1.2lf \n",L,dt,omega);
if (fabs(omega) > MAX_ROT_VEL) {
if (DEBUG_NAV) printf("Cannot reach point!\n");
return HUGE_VAL;
}
// double x_test = x_0 - r_c * sin(th_0) + r_c * sin(omega*dth + th_0);
// double y_test = y_0 - r_c * cos(th_0) - r_c * cos(omega*dth + th_0);
// printf("Goal ist at (%1.2lf,%1.2lf) --> Will end up at (%1.2lf, %1.2lf)\n",
// x_1,y_1,x_test,y_test);
double cost = ALPHA * dt + (1-ALPHA) * fabs(theta);
if (DEBUG_NAV) printf("Cost: %1.2lf\n",cost);
return cost;
}
float Pose::convertGlobalAngleToRelative(float globalAngle) const {
return norm_angle(globalAngle - angle);
}
float Pose::convertRelativeAngleToGlobal(float relativeAngle) const {
return norm_angle(angle + relativeAngle);
}
