bool CMOOSNavEngine::WrapAngleStates() { //wrap the state itself! int nStateNdx = m_pTracked->m_nStart+iiH; m_Xhat(nStateNdx,1) = MOOS_ANGLE_WRAP(m_Xhat(nStateNdx,1)); if(m_pTracked->GetEntityType()==CMOOSNavEntity::POSE_AND_RATE) { nStateNdx = m_pTracked->m_nStart+iiHdot; m_Xhat(nStateNdx,1) = MOOS_ANGLE_WRAP(m_Xhat(nStateNdx,1)); } return true; }
bool CXbowDriver::GetData() { //here we actually access serial ports etc if(m_pPort->IsStreaming()) { MOOSTrace("Crossbow must not be streaming\n"); return false; } //ask the crossbow for a reading m_pPort->Write("G",1); string sWhat; unsigned char Reply[CROSSBOW_POLLED_ANGLE_MODE_REPLY_LENGTH]; //note local call back invoked here to specify termination int nRead = m_pPort->ReadNWithTimeOut((char*)Reply,sizeof(Reply)); if(nRead == CROSSBOW_POLLED_ANGLE_MODE_REPLY_LENGTH) { if(Reply[0]!=255) { MOOSTrace("Unexpected Header in CrossBow reply\n"); } //angles short nRoll = (Reply[1]<<8) + Reply[2]; short nPitch = (Reply[3]<<8) + Reply[4]; short nYaw = (Reply[5]<<8) + Reply[6]; //This is roll, pitch and yaw for the DMU-AHRS, which is rotated //relative to the vehicle body. It has Z down and X out the tail. double df_deg_CBRoll = nRoll*180.0/pow(2.0,15.0); double df_deg_CBPitch = nPitch*180.0/pow(2.0,15.0); double df_deg_CBYaw = nYaw*180.0/pow(2.0,15.0); //account for alignment of crossbow in vehicle frame //this is the angle measured from the vehicle center line(y) double dfHeading = df_deg_CBYaw+m_dfVehicleYToINSX; //now correct for magnetic offset dfHeading+=m_dfMagneticOffset; //convert to Yaw.. //Notice that Heading is in DEGREES, positive CLOCKWISE //while Yaw is in RADIANS, positive COUNTERCLOCKWISE double dfYaw = -dfHeading*PI/180.0; dfYaw = MOOS_ANGLE_WRAP(dfYaw); //initialise transformed angles (in degrees) double df_deg_TRoll = df_deg_CBRoll; double df_deg_TPitch = df_deg_CBPitch; if( m_dfVehicleYToINSX == 0 ) { //Crossbow roll and vehicle body roll match. df_deg_TRoll = df_deg_TRoll; //same for Pitch. df_deg_TPitch = df_deg_TPitch; } else if (m_dfVehicleYToINSX == 180) { df_deg_TRoll = -df_deg_TRoll; df_deg_TPitch = -df_deg_TPitch; } else { MOOSTrace("Bad 'TWIST' value (use 0 or 180)!\r\n"); return false; } //look after pitch double dfPitch = MOOSDeg2Rad(df_deg_TPitch); //look after roll double dfRoll = MOOSDeg2Rad(df_deg_TRoll); //find the temperature every so often m_nTempCnt++; if((m_nTempCnt % 100) == 0) { short nTemp = ( Reply[25] << 8 ) + Reply[26]; m_dfTemp = 44.4 * ( ((double)nTemp * 5.0/4096.0) - 1.375); CMOOSMsg Temperature(MOOS_NOTIFY, "INS_TEMPERATURE", m_dfTemp); m_Notifications.push_back(Temperature); } //notify the MOOS CMOOSMsg Heading(MOOS_NOTIFY, "INS_HEADING", dfHeading); m_Notifications.push_back(Heading); CMOOSMsg Yaw(MOOS_NOTIFY, "INS_YAW", dfYaw); m_Notifications.push_back(Yaw); CMOOSMsg Pitch(MOOS_NOTIFY, "INS_PITCH", dfPitch); m_Notifications.push_back(Pitch); CMOOSMsg Roll(MOOS_NOTIFY, "INS_ROLL", dfRoll); m_Notifications.push_back(Roll); //rates short nRollRate = (Reply[7]<<8) + Reply[8];//p-dot short nPitchRate = (Reply[9]<<8) + Reply[10];//q-dot short nYawRate = (Reply[11]<<8) + Reply[12];//r-dot //xbow calib constant double RR = 100.0;//deg/sec double dfRollRate = nRollRate * RR * 1.5/pow(2.0,15); double dfPitchRate = nPitchRate * RR * 1.5/pow(2.0,15); double dfYawRate = nYawRate * RR * 1.5/pow(2.0,15); //note X and Y axes are switched CMOOSMsg RollRate(MOOS_NOTIFY, "INS_ROLLRATE_Y", dfRollRate); m_Notifications.push_back(RollRate); CMOOSMsg PitchRate(MOOS_NOTIFY, "INS_ROLLRATE_X", dfPitchRate); m_Notifications.push_back(PitchRate); CMOOSMsg YawRate(MOOS_NOTIFY, "INS_ROLLRATE_Z", dfYawRate); m_Notifications.push_back(YawRate); //accelerations short nXAccel = (Reply[13]<<8) + Reply[14];//v-dot short nYAccel = (Reply[15]<<8) + Reply[16];//u-dot short nZAccel = (Reply[17]<<8) + Reply[18];//w-dot //xbow calib constant double GR = 2.0;//G's double dfXAccel = nXAccel * GR * 1.5/pow(2.0,15); double dfYAccel = nYAccel * GR * 1.5/pow(2.0,15); double dfZAccel = nZAccel * GR * 1.5/pow(2.0,15); //note X and Y axes are switched CMOOSMsg XAccel(MOOS_NOTIFY, "INS_ACCEL_Y", dfXAccel); m_Notifications.push_back(XAccel); CMOOSMsg YAccel(MOOS_NOTIFY, "INS_ACCEL_X", dfYAccel); m_Notifications.push_back(YAccel); CMOOSMsg ZAccel(MOOS_NOTIFY, "INS_ACCEL_Z", dfZAccel); m_Notifications.push_back(ZAccel); //magnetic field data short nXMag = (Reply[19]<<8) + Reply[20]; short nYMag = (Reply[21]<<8) + Reply[22]; short nZMag = (Reply[23]<<8) + Reply[24]; double dfGaussX = nXMag * 1.25 * 1.5/pow(2.0,15.0); double dfGaussY = nYMag * 1.25 * 1.5/pow(2.0,15.0); double dfGaussZ = nZMag * 1.25 * 1.5/pow(2.0,15.0); CMOOSMsg GaussX(MOOS_NOTIFY, "INS_MAG_Y", dfGaussX); m_Notifications.push_back(GaussX); CMOOSMsg GaussY(MOOS_NOTIFY, "INS_MAG_X", dfGaussY); m_Notifications.push_back(GaussY); CMOOSMsg GaussZ(MOOS_NOTIFY, "INS_MAG_Z", dfGaussZ); m_Notifications.push_back(GaussZ); if(m_pPort->IsVerbose()) { //this allows us to print data in verbose mode //when teh port couldn't as it is verbose MOOSTrace("Roll = %7.3f Pitch = %7.3f Yaw = %7.3f Heading = %7.3f Temp = %3.2f\n", df_deg_TRoll, df_deg_TPitch, dfYaw*180/PI, dfHeading, m_dfTemp); } } else { MOOSTrace("read %d byte while expecting %d\n", nRead, CROSSBOW_POLLED_ANGLE_MODE_REPLY_LENGTH); } return true; }
bool WallFollowing::Iterate() { float angle = 0.0, coefficient_affichage = 45.0/*8.*/; float distance = 0.0, taille_pointeur; m_iterations++; m_map = Mat::zeros(LARGEUR_MAPPING, HAUTEUR_MAPPING, CV_8UC3); m_map = Scalar(255, 255, 255); std::vector<Point2f> points_obstacles; for(list<pair<float, float> >::const_iterator it = m_obstacles.begin() ; it != m_obstacles.end() ; it ++) { angle = it->first; // clef distance = it->second; // valeur //if(distance < 5.) if(distance < 0.25 || distance > 2.) continue; float x_obstacle = 0; float y_obstacle = 0; y_obstacle -= distance * cos(angle * M_PI / 180.0); x_obstacle += distance * sin(angle * M_PI / 180.0); // Filtrage des angles double angle_degre = MOOSRad2Deg(MOOS_ANGLE_WRAP(MOOSDeg2Rad(angle))); if(angle_degre > -160. && angle_degre < -70.) { points_obstacles.push_back(Point2f(x_obstacle, y_obstacle)); x_obstacle *= -coefficient_affichage; y_obstacle *= coefficient_affichage; x_obstacle += LARGEUR_MAPPING / 2.0; y_obstacle += HAUTEUR_MAPPING / 2.0; // Pointeurs taille_pointeur = 3; line(m_map, Point(x_obstacle, y_obstacle - taille_pointeur), Point(x_obstacle, y_obstacle + taille_pointeur), Scalar(161, 149, 104), 1, 8, 0); line(m_map, Point(x_obstacle - taille_pointeur, y_obstacle), Point(x_obstacle + taille_pointeur, y_obstacle), Scalar(161, 149, 104), 1, 8, 0); } } int echelle_ligne = 150; Mat m(points_obstacles); if(!points_obstacles.empty()) { Vec4f resultat_regression; try { // Méthode 1 fitLine(m, resultat_regression, CV_DIST_L2, 0, 0.01, 0.01); float x0 = resultat_regression[2]; float y0 = resultat_regression[3]; float vx = resultat_regression[0]; float vy = resultat_regression[1]; // Affichage de l'approximation line(m_map, Point((LARGEUR_MAPPING / 2.0) - (x0 * coefficient_affichage) + (vx * echelle_ligne), (HAUTEUR_MAPPING / 2.0) + (y0 * coefficient_affichage) - (vy * echelle_ligne)), Point((LARGEUR_MAPPING / 2.0) - (x0 * coefficient_affichage) - (vx * echelle_ligne), (HAUTEUR_MAPPING / 2.0) + (y0 * coefficient_affichage) + (vy * echelle_ligne)), Scalar(29, 133, 217), 1, 8, 0); // Orange // Méthode 2 fitLine(m, resultat_regression, CV_DIST_L12, 0, 0.01, 0.01); x0 = resultat_regression[2]; y0 = resultat_regression[3]; vx = resultat_regression[0]; vy = resultat_regression[1]; // Affichage de l'approximation line(m_map, Point((LARGEUR_MAPPING / 2.0) - (x0 * coefficient_affichage) + (vx * echelle_ligne), (HAUTEUR_MAPPING / 2.0) + (y0 * coefficient_affichage) - (vy * echelle_ligne)), Point((LARGEUR_MAPPING / 2.0) - (x0 * coefficient_affichage) - (vx * echelle_ligne), (HAUTEUR_MAPPING / 2.0) + (y0 * coefficient_affichage) + (vy * echelle_ligne)), Scalar(77, 130, 27), 1, 8, 0); // Vert // Méthode 3 fitLine(m, resultat_regression, CV_DIST_L1, 0, 0.01, 0.01); x0 = resultat_regression[2]; y0 = resultat_regression[3]; vx = resultat_regression[0]; vy = resultat_regression[1]; // Affichage de l'approximation line(m_map, Point((LARGEUR_MAPPING / 2.0) - (x0 * coefficient_affichage) + (vx * echelle_ligne), (HAUTEUR_MAPPING / 2.0) + (y0 * coefficient_affichage) - (vy * echelle_ligne)), Point((LARGEUR_MAPPING / 2.0) - (x0 * coefficient_affichage) - (vx * echelle_ligne), (HAUTEUR_MAPPING / 2.0) + (y0 * coefficient_affichage) + (vy * echelle_ligne)), Scalar(13, 13, 188), 1, 8, 0); // Rouge // Affichage de l'origine taille_pointeur = 6; line(m_map, Point((LARGEUR_MAPPING / 2.0) - (x0 * coefficient_affichage), (HAUTEUR_MAPPING / 2.0) + (y0 * coefficient_affichage) - taille_pointeur), Point((LARGEUR_MAPPING / 2.0) - (x0 * coefficient_affichage), (HAUTEUR_MAPPING / 2.0) + (y0 * coefficient_affichage) + taille_pointeur), Scalar(9, 0, 130), 2, 8, 0); line(m_map, Point((LARGEUR_MAPPING / 2.0) - (x0 * coefficient_affichage) - taille_pointeur, (HAUTEUR_MAPPING / 2.0) + (y0 * coefficient_affichage)), Point((LARGEUR_MAPPING / 2.0) - (x0 * coefficient_affichage) + taille_pointeur, (HAUTEUR_MAPPING / 2.0) + (y0 * coefficient_affichage)), Scalar(9, 0, 130), 2, 8, 0); angle = atan2(vy, vx); cout << "X0 : " << x0 << "\t\tY0 : " << y0 << endl; distance = abs(-vy*x0 + vx*y0); cout << "Angle : " << angle * 180.0 / M_PI << "\t\tDist : " << distance << endl; m_Comms.Notify("DIST_MUR", distance); if(m_regulate) computeAndSendCommands(angle, distance); } catch(Exception e) { } // Rotation Point2f src_center(m_map.cols/2.0F, m_map.rows/2.0F); Mat rot_mat = getRotationMatrix2D(src_center, 180.0, 1.0); warpAffine(m_map, m_map, rot_mat, m_map.size()); } // Affichage des échelles circulaires char texte[50]; float taille_texte = 0.4; Scalar couleur_echelles(220, 220, 220); for(float j = 1.0 ; j < 30.0 ; j ++) { float rayon = coefficient_affichage * j; circle(m_map, Point(LARGEUR_MAPPING / 2, HAUTEUR_MAPPING / 2), rayon, couleur_echelles, 1); sprintf(texte, "%dm", (int)j); rayon *= cos(M_PI / 4.0); putText(m_map, string(texte), Point((LARGEUR_MAPPING / 2) + rayon, (HAUTEUR_MAPPING / 2) - rayon), FONT_HERSHEY_SIMPLEX, taille_texte, couleur_echelles); } // Affichage de l'origine taille_pointeur = 20; line(m_map, Point(LARGEUR_MAPPING / 2, HAUTEUR_MAPPING / 2 - taille_pointeur * 1.5), Point(LARGEUR_MAPPING / 2, HAUTEUR_MAPPING / 2 + taille_pointeur), Scalar(150, 150, 150), 1, 8, 0); line(m_map, Point(LARGEUR_MAPPING / 2 - taille_pointeur, HAUTEUR_MAPPING / 2), Point(LARGEUR_MAPPING / 2 + taille_pointeur, HAUTEUR_MAPPING / 2), Scalar(150, 150, 150), 1, 8, 0); // Localisation des points de données line(m_map, Point(0, (HAUTEUR_MAPPING / 2) + HAUTEUR_MAPPING * sin(MOOSDeg2Rad(-70.))), Point(LARGEUR_MAPPING / 2, HAUTEUR_MAPPING / 2), Scalar(150, 150, 150), 1, 8, 0); line(m_map, Point(0, (HAUTEUR_MAPPING / 2) - HAUTEUR_MAPPING * sin(MOOSDeg2Rad(-160.))), Point(LARGEUR_MAPPING / 2, HAUTEUR_MAPPING / 2), Scalar(150, 150, 150), 1, 8, 0); // Affichage d'informations if(!points_obstacles.empty()) { sprintf(texte, "Dist = %.2fm Angle = %.2f", distance, angle); putText(m_map, string(texte), Point(10, HAUTEUR_MAPPING - 10), FONT_HERSHEY_SIMPLEX, taille_texte, Scalar(50, 50, 50)); } imshow("Mapping", m_map); waitKey(1); return(true); }