bool WayPointsViewer::Iterate()
{
m_iterations ++;
Mat map = m_map_background.clone();
// Position actuelle
tracerPointeur(m_map_background,
m_centre_ile_px.x + metresEnPixels(m_position.x),
m_centre_ile_px.y - metresEnPixels(m_position.y),
0, 1, Scalar(57, 221, 251)); // trace
tracerPointeur(map,
m_centre_ile_px.x + metresEnPixels(m_position.x),
m_centre_ile_px.y - metresEnPixels(m_position.y),
8, 1, Scalar(57, 221, 251)); // curseur
double heading_map = 180 - m_heading;
double longueur_ligne = metresEnPixels(30.);
line(map,
Point(m_centre_ile_px.x + metresEnPixels(m_position.x),
m_centre_ile_px.y - metresEnPixels(m_position.y)),
Point(m_centre_ile_px.x + metresEnPixels(m_position.x) + cos(MOOSDeg2Rad(heading_map))*longueur_ligne,
m_centre_ile_px.y - metresEnPixels(m_position.y) - sin(MOOSDeg2Rad(heading_map))*longueur_ligne),
Scalar(200, 200, 200), 1, 8, 0);
imshow("Mapping", map);
waitKey(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);
}
