Skip to content

Commit

Permalink
Fix search pattern logic and bugs.
Browse files Browse the repository at this point in the history
  • Loading branch information
@ClayJay3
ClayJay3 committed Jan 3, 2025
1 parent 1e374fc commit 3db55b0
Show file tree
Hide file tree
Showing 8 changed files with 221 additions and 62 deletions.
18 changes: 9 additions & 9 deletions src/AutonomyConstants.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -225,7 +225,7 @@ namespace constants
// OpenCV ArUco detection config.
const cv::aruco::PredefinedDictionaryType ARUCO_DICTIONARY = cv::aruco::DICT_4X4_50; // The predefined ArUco dictionary to use for detections.
const float ARUCO_TAG_SIDE_LENGTH = 0.015f; // Size of the white borders around the tag.
const int ARUCO_VALIDATION_THRESHOLD = 5; // How many times does the tag need to be detected(hit) before being validated as an actual aruco tag.
const int ARUCO_VALIDATION_THRESHOLD = 20; // How many times does the tag need to be detected(hit) before being validated as an actual aruco tag.
const int ARUCO_UNVALIDATED_TAG_FORGET_THRESHOLD = 5; // How many times can an unvalidated tag be missing from frame before being forgotten.
const int ARUCO_VALIDATED_TAG_FORGET_THRESHOLD = 10; // How many times can a validated tag be missing from frame before being forgotten.
const double ARUCO_PIXEL_THRESHOLD = 175; // Pixel value threshold for pre-process threshold mask
Expand Down Expand Up @@ -296,8 +296,8 @@ namespace constants
///////////////////////////////////////////////////////////////////////////

// Approaching Marker State
const int APPROACH_MARKER_DETECT_ATTEMPTS_LIMIT = 5; // How many consecutive failed attempts at detecting a tag before giving up on marker.
const double APPROACH_MARKER_MOTOR_POWER = 0.5; // The amount of power the motors use when approaching the marker.
const int APPROACH_MARKER_DETECT_ATTEMPTS_LIMIT = 60; // How many consecutive failed attempts at detecting a tag before giving up on marker.
const double APPROACH_MARKER_MOTOR_POWER = 0.3; // The amount of power the motors use when approaching the marker.
const double APPROACH_MARKER_PROXIMITY_THRESHOLD = 2.0; // How close in meters the rover must be to the target marker before completing its approach.
const double APPROACH_MARKER_TF_CONFIDENCE_THRESHOLD = 0.5; // What is the minimal confidence necessary to consider a tensorflow tag as a target.

Expand All @@ -318,12 +318,12 @@ namespace constants
const bool REVERSE_MAINTAIN_HEADING = true; // Whether or not the rover should maintain heading while reversing.

// Search Pattern State
const double SEARCH_ANGULAR_STEP_DEGREES = 57; // The amount the angle is incremented in each iteration of the loop (degrees).
const double SEARCH_MAX_RADIUS = 25; // The maximum radius to cover in the search (meters).
const double SEARCH_STARTING_HEADING_DEGREES = 0; // The angle the rover is facing at the start of the search(degrees).
const double SEARCH_SPACING = 2; // The spacing between successive points in the spiral (meters).
const double SEARCH_WAYPOINT_PROXIMITY = 1; // How close a rover must be to a point to have it count as visited.
const double SEARCH_MOTOR_POWER = 0.3; // The amount of power the motors use when approaching the marker.
const double SEARCH_ANGULAR_STEP_DEGREES = 57.0; // The amount the angle is incremented in each iteration of the loop (degrees).
const double SEARCH_MAX_RADIUS = 21.0; // The maximum radius to cover in the search (meters).
const double SEARCH_SPIRAL_SPACING = 1.0; // The spacing between successive points in the spiral (meters).
const double SEARCH_ZIGZAG_SPACING = 1.0; // The spacing between successive points in the zigzag (meters).
const double SEARCH_WAYPOINT_PROXIMITY = 2.0; // How close a rover must be to a point to have it count as visited.
const double SEARCH_MOTOR_POWER = 0.5; // The amount of power the motors use when approaching the marker.

// Handler.
const int STATEMACHINE_MAX_IPS = 60; // The maximum number of iteration per second of the state machines main thread.
Expand Down
152 changes: 114 additions & 38 deletions src/algorithms/SearchPattern.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -38,8 +38,8 @@ namespace searchpattern
* @param dMaxRadius - The maximum radius to cover in the search (meters).
* @param dStartingHeadingDegrees - The angle the rover is facing at the start
* of the search (degrees).
* @param dSpacing - The spacing between successive points in the spiral
* (meters).
* @param dStartSpacing - The spacing between successive points in the spiral
* (meters). This will become larger as the spiral grows.
* @return vWaypoints - A vector representing the waypoints forming the spiral
* search pattern.
*
Expand All @@ -50,22 +50,23 @@ namespace searchpattern
const double dAngularStepDegrees = 57,
const double dMaxRadius = 25,
const double dStartingHeadingDegrees = 0,
const double dSpacing = 1)
const double dStartSpacing = 1)
{
// Define variables.
std::vector<geoops::Waypoint> vWaypoints;
double dAngularStepRadians = dAngularStepDegrees * M_PI / 180;
double dAngleRadians = (dStartingHeadingDegrees + 90) * M_PI / 180;
double dCurrentRadius = 0;
double dStartingX = stStartingPoint.dEasting;
double dStartingY = stStartingPoint.dNorthing;
double dAngularStepRadians = dAngularStepDegrees * M_PI / 180;
double dAngleRadians = (dStartingHeadingDegrees + 90) * M_PI / 180;
double dCurrentSpacingWindUp = 0.0;
double dStartingX = stStartingPoint.dEasting;
double dStartingY = stStartingPoint.dNorthing;
double dCurrentRadius = 0.0;

// Calculate each waypoint.
// Calculate each waypoint. Stop when the radius exceeds the maximum.
while (dCurrentRadius <= dMaxRadius)
{
// Get X and Y positions for the current point.
double dCurrentX = dStartingX + dCurrentRadius * cos(dAngleRadians);
double dCurrentY = dStartingY + dCurrentRadius * sin(dAngleRadians);
double dCurrentX = dStartingX + dCurrentSpacingWindUp * cos(dAngleRadians);
double dCurrentY = dStartingY + dCurrentSpacingWindUp * sin(dAngleRadians);

// Add the current waypoint to the final vector.
geoops::UTMCoordinate stCurrentCoordinate = stStartingPoint;
Expand All @@ -76,7 +77,10 @@ namespace searchpattern

// Increment angle and radius for the next waypoint.
dAngleRadians += dAngularStepRadians;
dCurrentRadius += dSpacing;
dCurrentSpacingWindUp += dStartSpacing;

// Calculate the current distance from the starting point. This is our radius.
dCurrentRadius = geoops::CalculateGeoMeasurement(stStartingPoint, stCurrentCoordinate).dDistanceMeters;
}

return vWaypoints;
Expand All @@ -91,7 +95,7 @@ namespace searchpattern
* @param dMaxRadius - The maximum radius to cover in the search (meters).
* @param dStartingHeadingDegrees - The angle the rover is facing at the start
* of the search (degrees).
* @param dSpacing - The spacing between successive points in the spiral
* @param dStartSpacing - The spacing between successive points in the spiral
* (meters).
* @return vWaypoints - A vector representing the waypoints forming the spiral
* search pattern.
Expand All @@ -103,23 +107,24 @@ namespace searchpattern
const double dAngularStepDegrees = 57,
const double dMaxRadius = 25,
const double dStartingHeadingDegrees = 0,
const double dSpacing = 1)
const double dStartSpacing = 1)
{
// Define variables.
std::vector<geoops::Waypoint> vWaypoints;
geoops::UTMCoordinate stStartingPointUTM = geoops::ConvertGPSToUTM(stStartingPoint);
double dAngularStepRadians = dAngularStepDegrees * M_PI / 180;
double dAngleRadians = (dStartingHeadingDegrees + 90) * M_PI / 180;
double dCurrentRadius = 0;
double dCurrentSpacingWindUp = 0.0;
double dStartingX = stStartingPointUTM.dEasting;
double dStartingY = stStartingPointUTM.dNorthing;
double dCurrentRadius = 0.0;

// Calculate each waypoint.
// Calculate each waypoint. Stop when the radius exceeds the maximum.
while (dCurrentRadius <= dMaxRadius)
{
// Get X and Y positions for the current point.
double dCurrentX = dStartingX + dCurrentRadius * cos(dAngleRadians);
double dCurrentY = dStartingY + dCurrentRadius * sin(dAngleRadians);
double dCurrentX = dStartingX + dCurrentSpacingWindUp * cos(dAngleRadians);
double dCurrentY = dStartingY + dCurrentSpacingWindUp * sin(dAngleRadians);

// Add the current waypoint to the final vector.
geoops::UTMCoordinate stCurrentCoordinate = stStartingPointUTM;
Expand All @@ -130,7 +135,10 @@ namespace searchpattern

// Increment angle and radius for the next waypoint.
dAngleRadians += dAngularStepRadians;
dCurrentRadius += dSpacing;
dCurrentSpacingWindUp += dStartSpacing;

// Calculate the current distance from the starting point. This is our radius.
dCurrentRadius = geoops::CalculateGeoMeasurement(stStartingPointUTM, stCurrentCoordinate).dDistanceMeters;
}

return vWaypoints;
Expand Down Expand Up @@ -161,11 +169,22 @@ namespace searchpattern
{
// Create instance variables.
std::vector<geoops::Waypoint> vWaypoints;
double dStartingX = stCenterPoint.dEasting - (dWidth / 2);
double dStartingY = stCenterPoint.dNorthing - (dHeight / 2);
double dCurrentX = dStartingX;
double dCurrentY = dStartingY;
bool bZigNotZag = true;
double dStartingX = stCenterPoint.dEasting - (dWidth / 2);
double dStartingY = stCenterPoint.dNorthing - (dHeight / 2);
double dCurrentX = dStartingX;
double dCurrentY = dStartingY;
bool bZigNotZag = true;
double bCalcSpacing = dSpacing;

// Limit spacing to the width or height.
if (bCalcSpacing > dWidth)
{
bCalcSpacing = dWidth;
}
if (bCalcSpacing > dHeight)
{
bCalcSpacing = dHeight;
}

// Loop until covered entire space of width and height.
while ((bVertical && dCurrentY <= dStartingY + dHeight) || (!bVertical && dCurrentX <= dStartingX + dWidth))
Expand Down Expand Up @@ -216,31 +235,54 @@ namespace searchpattern
if (bVertical)
{
// Increment current position.
dCurrentX += bZigNotZag ? dSpacing : -dSpacing;
dCurrentX += bZigNotZag ? bCalcSpacing : -bCalcSpacing;
}
else
{
// Increment current position.
dCurrentY += bZigNotZag ? dSpacing : -dSpacing;
dCurrentY += bZigNotZag ? bCalcSpacing : -bCalcSpacing;
}
}

// Now shift the opposite coordinate forward.
if (bVertical)
{
dCurrentY += dSpacing;
dCurrentY += bCalcSpacing;
}
else
{
dCurrentX += dSpacing;
dCurrentX += bCalcSpacing;
}

// Toggle zigzag direction.
bZigNotZag = !bZigNotZag;
}

// The path now contains the zigzag pattern with points spaced at the specified distance. This means that there
// are many points potentially very close to each other. This is not ideal for navigation, so we will filter out
// points that are too close to each other, by calculating their GeoMeasurement and testing if the distance is greater
// then the CalcSpacing. If the CalcSpacing is greater a certain threshold, we will remove the point since it jumps to the other side.
// This will reduce the number of points in the zigzag pattern, making it more efficient for navigation.

// Create a vector to store the filtered waypoints.
std::vector<geoops::Waypoint> vFilterWaypoints;
// Always store the first point.
vFilterWaypoints.push_back(vWaypoints[0]);
// Loop through the waypoints and remove any that are too close to each other.
for (size_t i = 0; i < vWaypoints.size() - 1; ++i)
{
// Calculate the GeoMeasurement between the current waypoint and the next waypoint.
geoops::GeoMeasurement stGeoMeasurement = geoops::CalculateGeoMeasurement(vWaypoints[i].GetGPSCoordinate(), vWaypoints[i + 1].GetGPSCoordinate());
if (stGeoMeasurement.dDistanceMeters > bCalcSpacing * 1.5)
{
// Add the points to the filtered waypoints.
vFilterWaypoints.push_back(vWaypoints[i]);
vFilterWaypoints.push_back(vWaypoints[i + 1]);
}
}

// Return the final path.
return vWaypoints;
return vFilterWaypoints;
}

/******************************************************************************
Expand Down Expand Up @@ -274,6 +316,17 @@ namespace searchpattern
double dCurrentX = dStartingX;
double dCurrentY = dStartingY;
bool bZigNotZag = true;
double bCalcSpacing = dSpacing;

// Limit spacing to the width or height.
if (bCalcSpacing > dWidth)
{
bCalcSpacing = dWidth;
}
if (bCalcSpacing > dHeight)
{
bCalcSpacing = dHeight;
}

// Loop until covered entire space of width and height.
while ((bVertical && dCurrentY <= dStartingY + dHeight) || (!bVertical && dCurrentX <= dStartingX + dWidth))
Expand All @@ -285,12 +338,12 @@ namespace searchpattern
if (bZigNotZag)
{
// Zig.
dCurrentX = dStartingX + dSpacing;
dCurrentX = dStartingX + bCalcSpacing;
}
else
{
// Zag.
dCurrentX = dStartingX - dSpacing;
dCurrentX = dStartingX - bCalcSpacing;
}
}
else
Expand All @@ -299,12 +352,12 @@ namespace searchpattern
if (bZigNotZag)
{
// Zig.
dCurrentY = dStartingY + dSpacing;
dCurrentY = dStartingY + bCalcSpacing;
}
else
{
// Zag.
dCurrentY = dStartingY - dSpacing;
dCurrentY = dStartingY - bCalcSpacing;
}
}

Expand All @@ -324,31 +377,54 @@ namespace searchpattern
if (bVertical)
{
// Increment current position.
dCurrentX += bZigNotZag ? dSpacing : -dSpacing;
dCurrentX += bZigNotZag ? bCalcSpacing : -bCalcSpacing;
}
else
{
// Increment current position.
dCurrentY += bZigNotZag ? dSpacing : -dSpacing;
dCurrentY += bZigNotZag ? bCalcSpacing : -bCalcSpacing;
}
}

// Now shift the opposite coordinate forward.
if (bVertical)
{
dCurrentY += dSpacing;
dCurrentY += bCalcSpacing;
}
else
{
dCurrentX += dSpacing;
dCurrentX += bCalcSpacing;
}

// Toggle zigzag direction.
bZigNotZag = !bZigNotZag;
}

// The path now contains the zigzag pattern with points spaced at the specified distance. This means that there
// are many points potentially very close to each other. This is not ideal for navigation, so we will filter out
// points that are too close to each other, by calculating their GeoMeasurement and testing if the distance is greater
// then the CalcSpacing. If the CalcSpacing is greater a certain threshold, we will remove the point since it jumps to the other side.
// This will reduce the number of points in the zigzag pattern, making it more efficient for navigation.

// Create a vector to store the filtered waypoints.
std::vector<geoops::Waypoint> vFilterWaypoints;
// Always store the first point.
vFilterWaypoints.push_back(vWaypoints[0]);
// Loop through the waypoints and remove any that are too close to each other.
for (size_t i = 0; i < vWaypoints.size() - 1; ++i)
{
// Calculate the GeoMeasurement between the current waypoint and the next waypoint.
geoops::GeoMeasurement stGeoMeasurement = geoops::CalculateGeoMeasurement(vWaypoints[i].GetGPSCoordinate(), vWaypoints[i + 1].GetGPSCoordinate());
if (stGeoMeasurement.dDistanceMeters > bCalcSpacing * 1.5)
{
// Add the points to the filtered waypoints.
vFilterWaypoints.push_back(vWaypoints[i]);
vFilterWaypoints.push_back(vWaypoints[i + 1]);
}
}

// Return the final path.
return vWaypoints;
return vFilterWaypoints;
}
} // namespace searchpattern
#endif
2 changes: 1 addition & 1 deletion src/handlers/WaypointHandler.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -857,7 +857,7 @@ geoops::RoverPose WaypointHandler::SmartRetrieveRoverPose(bool bVIOTracking)

// Assemble the error metrics into a single string. We are going to include the original GPS positions of the NavBoard and the Camera and then include the error. Same
// thing for the heading data.
std::string szErrorMetrics = "--------[ ZED MainCam Pose Tracking Error ]--------\nGPS Position Error (UTM for easy reading):\n" +
std::string szErrorMetrics = "--------[ ZED MainCam Pose Tracking Error ]--------\nGPS/VIO Position Error (UTM for easy reading):\n" +
std::to_string(ConvertGPSToUTM(stCurrentGPSPosition).dEasting) + " (NavBoard) vs. " + std::to_string(stCurrentUTMPosition.dEasting) +
" (Camera) = " + std::to_string(dEastingError) + " (error)\n" + std::to_string(ConvertGPSToUTM(stCurrentGPSPosition).dNorthing) +
" (NavBoard) vs. " + std::to_string(stCurrentUTMPosition.dNorthing) + " (Camera) = " + std::to_string(dNorthingError) + " (error)\n" +
Expand Down
Loading

0 comments on commit 3db55b0

Please sign in to comment.