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); }