The WaypointHandler class is used throughout the entire project (mainly by the state machine) to globally store a list of waypoints that the rover will navigate to. More...

#include <WaypointHandler.h>

Public Member Functions
	WaypointHandler ()
	Construct a new geoops::Waypoint Handler:: geoops::Waypoint Handler obstacle.

	~WaypointHandler ()
	Destroy the geoops::Waypoint Handler:: geoops::Waypoint Handler obstacle.

void	AddWaypoint (const geoops::Waypoint &stWaypoint)
	Append a waypoint to the end of the WaypointHandler's list.

void	AddWaypoint (const geoops::GPSCoordinate &stLocation, const geoops::WaypointType &eType, const double dRadius=0.0)
	Append a waypoint to the end of the WaypointHandler's list.

void	AddWaypoint (const geoops::UTMCoordinate &stLocation, const geoops::WaypointType &eType, const double dRadius=0.0)
	Append a waypoint to the end of the WaypointHandler's list.

void	StorePath (const std::string &szPathName, const std::vector< geoops::Waypoint > &vWaypointPath)
	Store a path in the WaypointHandler.

void	StorePath (const std::string &szPathName, const std::vector< geoops::GPSCoordinate > &vLocationPath)
	Store a path in the WaypointHandler.

void	StorePath (const std::string &szPathName, const std::vector< geoops::UTMCoordinate > &vLocationPath)
	Store a path in the WaypointHandler.

void	AddObstacle (const geoops::Waypoint &stWaypoint)
	Append a new obstacle to the WaypointHandler obstacle list.

void	AddObstacle (const geoops::GPSCoordinate &stLocation, const double dRadius=0.0)
	Append a new obstacle to the WaypointHandler obstacle list.

void	AddObstacle (const geoops::UTMCoordinate &stLocation, const double dRadius=0.0)
	Append a new obstacle to the WaypointHandler obstacle list.

void	DeleteWaypoint (const long unsigned int nIndex)
	Delete the geoops::Waypoint at a given index from the waypoint handler.

void	DeleteWaypoint (const geoops::Waypoint &stWaypoint)
	Delete a waypoint from the WaypointHandler given a matching waypoint. Any waypoint in the list that matches the given one will be removed.

void	DeleteWaypoint (const geoops::GPSCoordinate &stLocation)
	Delete a waypoint from the WaypointHandler given a matching location. Any waypoint in the list that matches the given location will be removed.

void	DeleteWaypoint (const geoops::UTMCoordinate &stLocation)
	Delete a waypoint from the WaypointHandler given a matching location. Any waypoint in the list that matches the given location will be removed.

bool	DeletePath (const std::string &szPathName)
	Delete the path vector stored at the given key.

void	DeleteObstacle (const long unsigned int nIndex)
	Delete the obstacle at a given index from the waypoint handler obstacle list.

void	DeleteObstacle (const geoops::Waypoint &stWaypoint)
	Delete an obstacle from the WaypointHandler given a matching location. Any waypoint in the list that matches the given location will be removed.

void	DeleteObstacle (const geoops::GPSCoordinate &stLocation)
	Delete an obstacle from the WaypointHandler given a matching location. Any waypoint in the list that matches the given location will be removed.

void	DeleteObstacle (const geoops::UTMCoordinate &stLocation)
	Delete an obstacle from the WaypointHandler given a matching location. Any waypoint in the list that matches the given location will be removed.

void	ClearWaypoints ()
	Clears/deletes all Waypoints stored in the WaypointHandler.

void	ClearPaths ()
	Clears/deletes all keys and paths store in the WaypointHandler.

void	ClearObstacles ()
	Clears/deletes all permanent objects stored in the WaypointHandler.

geoops::Waypoint	PopNextWaypoint ()
	Removes and returns the next waypoint at the front of the list.

const geoops::Waypoint	PeekNextWaypoint ()
	Returns an immutable reference to the geoops::Waypoint struct at the front of the list without removing it.

const geoops::Waypoint	RetrieveWaypointAtIndex (const long unsigned int nIndex)
	Retrieve an immutable reference to the waypoint at the given index.

const std::vector< geoops::Waypoint >	RetrievePath (const std::string &szPathName)
	Retrieve an immutable reference to the path at the given path name/key.

const geoops::Waypoint	RetrieveObstacleAtIndex (const long unsigned int nIndex)
	Retrieve an immutable reference to the obstacle at the given index.

const std::vector< geoops::Waypoint >	GetAllWaypoints ()
	Accessor for the full list of current waypoints stored in the WaypointHandler.

const std::vector< geoops::Waypoint >	GetAllObstacles ()
	Accessor for the full list of current obstacle stored in the WaypointHandler.

int	GetWaypointCount ()
	Accessor for the number of elements on the WaypointHandler's waypoint vector.

int	GetPathsCount ()
	Accessor for the number of paths stored in the WaypointHandler.

int	GetObstaclesCount ()
	Accessor for the number of elements on the WaypointHandler's obstacle vector.

geoops::RoverPose	SmartRetrieveRoverPose (bool bVIOHeading=true, bool bVIOTracking=false)
	Retrieve the rover's current position and heading. Automatically picks between getting the position/heading from the NavBoard, ZEDSDK Fusion module, or ZED positional tracking. In most cases, this will be the method that should be called over getting the data directly from NavBoard.

double	SmartRetrieveVelocity ()
	Retrieve the rover's current velocity. Currently there is no easy way to get the velocity of the ZEDCam so this method just returns the GPS-based velocity.

double	SmartRetrieveAngularVelocity ()
	Retrieve the rover's current velocity. Currently there is no easy way to get the velocity of the ZEDCam so this method just returns the GPS-based velocity.

Private Attributes
std::vector< geoops::Waypoint >	m_vWaypointList

std::shared_mutex	m_muWaypointsMutex

std::unordered_map< std::string, std::vector< geoops::Waypoint > >	m_umStoredPaths

std::shared_mutex	m_muPathMutex

std::vector< geoops::Waypoint >	m_vPermanentObstacles

std::shared_mutex	m_muObstaclesMutex

const std::function< void(const rovecomm::RoveCommPacket< double > &, const sockaddr_in &)>	AddPositionLegCallback
	Callback function that is called whenever RoveComm receives new ADDPOSITIONLEG packet.

const std::function< void(const rovecomm::RoveCommPacket< double > &, const sockaddr_in &)>	AddMarkerLegCallback
	Callback function that is called whenever RoveComm receives new ADDMARKERLEG packet.

const std::function< void(const rovecomm::RoveCommPacket< double > &, const sockaddr_in &)>	AddObjectLegCallback
	Callback function that is called whenever RoveComm receives new ADDOBJECTLEG packet.

const std::function< void(const rovecomm::RoveCommPacket< double > &, const sockaddr_in &)>	AddObstacleCallback
	Callback function that is called whenever RoveComm receives new ADDOBSTACLE packet.

const std::function< void(const rovecomm::RoveCommPacket< uint8_t > &, const sockaddr_in &)>	ClearWaypointsCallback
	Callback function that is called whenever RoveComm receives new CLEARWAYPOINTS packet.

const std::function< void(const rovecomm::RoveCommPacket< uint8_t > &, const sockaddr_in &)>	ClearObstaclesCallback
	Callback function that is called whenever RoveComm receives new CLEAROBSTACLES packet.

Detailed Description

The WaypointHandler class is used throughout the entire project (mainly by the state machine) to globally store a list of waypoints that the rover will navigate to.

Author: clayjay3 (clayt.nosp@m.onra.nosp@m.ycowe.nosp@m.n@gm.nosp@m.ail.c.nosp@m.om)

Date: 2024-02-02

Constructor & Destructor Documentation

◆ WaypointHandler()

WaypointHandler::WaypointHandler ( )

Construct a new geoops::Waypoint Handler:: geoops::Waypoint Handler obstacle.

Author: clayjay3 (clayt.nosp@m.onra.nosp@m.ycowe.nosp@m.n@gm.nosp@m.ail.c.nosp@m.om)

Date: 2024-02-02

{
    // Set RoveComm callbacks.
    network::g_pRoveCommUDPNode->AddUDPCallback<double>(AddPositionLegCallback, manifest::Autonomy::COMMANDS.find("ADDPOSITIONLEG")->second.DATA_ID);
    network::g_pRoveCommUDPNode->AddUDPCallback<double>(AddMarkerLegCallback, manifest::Autonomy::COMMANDS.find("ADDMARKERLEG")->second.DATA_ID);
    network::g_pRoveCommUDPNode->AddUDPCallback<double>(AddObjectLegCallback, manifest::Autonomy::COMMANDS.find("ADDOBJECTLEG")->second.DATA_ID);
    network::g_pRoveCommUDPNode->AddUDPCallback<double>(AddObstacleCallback, manifest::Autonomy::COMMANDS.find("ADDOBSTACLE")->second.DATA_ID);
    network::g_pRoveCommUDPNode->AddUDPCallback<uint8_t>(ClearWaypointsCallback, manifest::Autonomy::COMMANDS.find("CLEARWAYPOINTS")->second.DATA_ID);
    network::g_pRoveCommUDPNode->AddUDPCallback<uint8_t>(ClearObstaclesCallback, manifest::Autonomy::COMMANDS.find("CLEAROBSTACLES")->second.DATA_ID);
}

◆ ~WaypointHandler()

WaypointHandler::~WaypointHandler ( )

Destroy the geoops::Waypoint Handler:: geoops::Waypoint Handler obstacle.

Author: clayjay3 (clayt.nosp@m.onra.nosp@m.ycowe.nosp@m.n@gm.nosp@m.ail.c.nosp@m.om)

Date: 2024-02-02

{
    // Nothing to destroy.
}

Member Function Documentation

◆ AddWaypoint() [1/3]

void WaypointHandler::AddWaypoint ( const geoops::Waypoint & stWaypoint )

Append a waypoint to the end of the WaypointHandler's list.

Parameters

stWaypoint - The WaypointHandler::geoops::Waypoint struct containing information about the waypoint to store in the handler.

Author: clayjay3 (clayt.nosp@m.onra.nosp@m.ycowe.nosp@m.n@gm.nosp@m.ail.c.nosp@m.om)

Date: 2024-02-02

{
    // Acquire a write lock on the waypoint vector.
    std::unique_lock<std::shared_mutex> lkWaypointListLock(m_muWaypointsMutex);
    // Add waypoint to end of member variable vector.
    m_vWaypointList.emplace_back(stWaypoint);
}

◆ AddWaypoint() [2/3]

void WaypointHandler::AddWaypoint	(	const geoops::GPSCoordinate &	stLocation,
		const geoops::WaypointType &	eType,
		const double	dRadius = `0.0`
	)

Append a waypoint to the end of the WaypointHandler's list.

Parameters

stLocation	- The location of the waypoint stored in a geoops namespace GPSCoordinate struct.
eType	- The leg type of the waypoint signalling if this is a tag, navigation, obstacle, etc. waypoint.
dRadius	- The circular area around the waypoint that should be counted as reaching the waypoint. Or obstacle radius.

Author: clayjay3 (clayt.nosp@m.onra.nosp@m.ycowe.nosp@m.n@gm.nosp@m.ail.c.nosp@m.om)

Date: 2024-02-03

{
    // Construct a new waypoint struct from the given info.
    geoops::Waypoint stTempWaypoint(stLocation, eType, dRadius);
 
    // Acquire a write lock on the waypoint vector.
    std::unique_lock<std::shared_mutex> lkWaypointListLock(m_muWaypointsMutex);
    // Add waypoint to end of member variable vector.
    m_vWaypointList.emplace_back(stTempWaypoint);
}

◆ AddWaypoint() [3/3]

void WaypointHandler::AddWaypoint	(	const geoops::UTMCoordinate &	stLocation,
		const geoops::WaypointType &	eType,
		const double	dRadius = `0.0`
	)

Append a waypoint to the end of the WaypointHandler's list.

Parameters

stLocation	- The location of the waypoint stored in a geoops namespace UTMCoordinate struct.
eType	- The leg type of the waypoint signalling if this is a tag, navigation, obstacle, etc. waypoint.
dRadius	- The circular area around the waypoint that should be counted as reaching the waypoint. Or obstacle radius.

Author: clayjay3 (clayt.nosp@m.onra.nosp@m.ycowe.nosp@m.n@gm.nosp@m.ail.c.nosp@m.om)

Date: 2024-02-03

{
    // Construct a new waypoint struct from the given info.
    geoops::Waypoint stTempWaypoint(stLocation, eType, dRadius);
 
    // Acquire a write lock on the waypoint vector.
    std::unique_lock<std::shared_mutex> lkWaypointListLock(m_muWaypointsMutex);
    // Add waypoint to end of member variable vector.
    m_vWaypointList.emplace_back(stTempWaypoint);
}

◆ StorePath() [1/3]

void WaypointHandler::StorePath	(	const std::string &	szPathName,
		const std::vector< geoops::Waypoint > &	vWaypointPath
	)

Store a path in the WaypointHandler.

Parameters

szPathName	- The key that will be used to store, and later reference, the path in the WaypointHandler.
vWaypointPath	- A vector containing geoops::Waypoint structs with data about each point on the path.

Author: clayjay3 (clayt.nosp@m.onra.nosp@m.ycowe.nosp@m.n@gm.nosp@m.ail.c.nosp@m.om)

Date: 2024-02-03

{
    // Acquire a write lock on the path unordered map.
    std::unique_lock<std::shared_mutex> lkPathsLock(m_muPathMutex);
    // Add vector of waypoint to map with the given string as a key.
    m_umStoredPaths[szPathName] = vWaypointPath;
}

◆ StorePath() [2/3]

void WaypointHandler::StorePath	(	const std::string &	szPathName,
		const std::vector< geoops::GPSCoordinate > &	vLocationPath
	)

Store a path in the WaypointHandler.

Parameters

szPathName	- The key that will be used to store, and later reference, the path in the WaypointHandler.
vWaypointPath	- A vector containing GPSCoordinate structs containing location data about each point in the path.

Note: Paths must be stored in the WaypointHandler as a vector is geoops::Waypoint structs. This will create a new geoops::Waypoint struct for each GPSCoordinate and use a default type of eNavigationWaypoint with a radius of 0.0.

Author: clayjay3 (clayt.nosp@m.onra.nosp@m.ycowe.nosp@m.n@gm.nosp@m.ail.c.nosp@m.om)

Date: 2024-02-03

{
    // Create instance variables.
    std::vector<geoops::Waypoint> vWaypointPath;
 
    // Loop through each GPSCoordinate in the given vector and repack the info into a geoops::Waypoint.
    for (geoops::GPSCoordinate stLocation : vLocationPath)
    {
        // Create a new waypoint and store location info in it.
        geoops::Waypoint stWaypoint(stLocation, geoops::WaypointType::eNavigationWaypoint);
 
        // Append waypoint to the temporary waypoint path.
        vWaypointPath.emplace_back(stWaypoint);
    }
 
    // Acquire a write lock on the path unordered map.
    std::unique_lock<std::shared_mutex> lkPathsLock(m_muPathMutex);
    // Add vector of waypoint to map with the given string as a key.
    m_umStoredPaths[szPathName] = vWaypointPath;
}

◆ StorePath() [3/3]

void WaypointHandler::StorePath	(	const std::string &	szPathName,
		const std::vector< geoops::UTMCoordinate > &	vLocationPath
	)

Store a path in the WaypointHandler.

Parameters

szPathName	- The key that will be used to store, and later reference, the path in the WaypointHandler.
vWaypointPath	- A vector containing UTMCoordinate structs containing location data about each point in the path.

Note: Paths must be stored in the WaypointHandler as a vector is geoops::Waypoint structs. This will create a new geoops::Waypoint struct for each UTMCoordinate and use a default type of eNavigationWaypoint with a radius of 0.0.

Author: clayjay3 (clayt.nosp@m.onra.nosp@m.ycowe.nosp@m.n@gm.nosp@m.ail.c.nosp@m.om)

Date: 2024-02-03

{
    // Create instance variables.
    std::vector<geoops::Waypoint> vWaypointPath;
 
    // Loop through each UTMCoordinate in the given vector and repack the info into a geoops::Waypoint.
    for (geoops::UTMCoordinate stLocation : vLocationPath)
    {
        // Create a new waypoint and store location info in it.
        geoops::Waypoint stWaypoint(stLocation, geoops::WaypointType::eNavigationWaypoint);
 
        // Append waypoint to the temporary waypoint path.
        vWaypointPath.emplace_back(stWaypoint);
    }
 
    // Acquire a write lock on the path unordered map.
    std::unique_lock<std::shared_mutex> lkPathsLock(m_muPathMutex);
    // Add vector of waypoint to map with the given string as a key.
    m_umStoredPaths[szPathName] = vWaypointPath;
}

◆ AddObstacle() [1/3]

void WaypointHandler::AddObstacle ( const geoops::Waypoint & stWaypoint )

Append a new obstacle to the WaypointHandler obstacle list.

Parameters

stWaypoint - The WaypointHandler::geoops::Waypoint struct containing information about the obstacle to store in the handler.

Author: clayjay3 (clayt.nosp@m.onra.nosp@m.ycowe.nosp@m.n@gm.nosp@m.ail.c.nosp@m.om)

Date: 2024-02-03

{
    // Acquire a write lock on the obstacle vector.
    std::unique_lock<std::shared_mutex> lkObjectListLock(m_muObstaclesMutex);
    // Add obstacle waypoint to end of member variable vector.
    m_vPermanentObstacles.emplace_back(stWaypoint);
}

◆ AddObstacle() [2/3]

void WaypointHandler::AddObstacle	(	const geoops::GPSCoordinate &	stLocation,
		const double	dRadius = `0.0`
	)

Append a new obstacle to the WaypointHandler obstacle list.

Parameters

stLocation	- The location of the waypoint stored in a geoops namespace GPSCoordinate struct.
dRadius	- The circular area around the obstacle or the obstacle radius.

Author: clayjay3 (clayt.nosp@m.onra.nosp@m.ycowe.nosp@m.n@gm.nosp@m.ail.c.nosp@m.om)

Date: 2024-02-03

{
    // Construct a new waypoint struct from the given info.
    geoops::Waypoint stTempWaypoint(stLocation, geoops::WaypointType::eObstacleWaypoint, dRadius);
 
    // Acquire a write lock on the waypoint vector.
    std::unique_lock<std::shared_mutex> lkObjectListLock(m_muObstaclesMutex);
    // Add waypoint to end of member variable vector.
    m_vPermanentObstacles.emplace_back(stTempWaypoint);
}

◆ AddObstacle() [3/3]

void WaypointHandler::AddObstacle	(	const geoops::UTMCoordinate &	stLocation,
		const double	dRadius = `0.0`
	)

Append a new obstacle to the WaypointHandler obstacle list.

Parameters

stLocation	- The location of the waypoint stored in a geoops namespace UTMCoordinate struct.
dRadius	- The circular area around the obstacle or the obstacle radius.

Author: clayjay3 (clayt.nosp@m.onra.nosp@m.ycowe.nosp@m.n@gm.nosp@m.ail.c.nosp@m.om)

Date: 2024-02-03

{
    // Construct a new waypoint struct from the given info.
    geoops::Waypoint stTempWaypoint(stLocation, geoops::WaypointType::eObstacleWaypoint, dRadius);
 
    // Acquire a write lock on the waypoint vector.
    std::unique_lock<std::shared_mutex> lkObjectListLock(m_muObstaclesMutex);
    // Add waypoint to end of member variable vector.
    m_vPermanentObstacles.emplace_back(stTempWaypoint);
}

◆ DeleteWaypoint() [1/4]

void WaypointHandler::DeleteWaypoint ( const long unsigned int nIndex )

Delete the geoops::Waypoint at a given index from the waypoint handler.

Parameters

nIndex - The index of the element to remove.

Author: clayjay3 (clayt.nosp@m.onra.nosp@m.ycowe.nosp@m.n@gm.nosp@m.ail.c.nosp@m.om)

Date: 2024-02-03

{
    // Acquire a read lock on the waypoint vector.
    std::shared_lock<std::shared_mutex> lkWaypointListLock(m_muWaypointsMutex);
    // Check if the vector has at least one waypoint.
    if (nIndex < m_vWaypointList.size())
    {
        // Release read lock.
        lkWaypointListLock.unlock();
 
        // Acquire a write lock on the waypoint vector.
        std::unique_lock<std::shared_mutex> lkWaypointListLock(m_muWaypointsMutex);
        // Delete the geoops::Waypoint at the index.
        m_vWaypointList.erase(m_vWaypointList.begin() + nIndex);
    }
    else
    {
        // Submit logger message.
        LOG_ERROR(logging::g_qSharedLogger,
                  "Attempted to delete a waypoint at index {} from the WaypointHandler but it is already empty or the index is out of bounds!",
                  nIndex);
    }
}

◆ DeleteWaypoint() [2/4]

void WaypointHandler::DeleteWaypoint ( const geoops::Waypoint & stWaypoint )

Delete a waypoint from the WaypointHandler given a matching waypoint. Any waypoint in the list that matches the given one will be removed.

Parameters

stWaypoint - The equivalent waypoint that should be removed.

Author: clayjay3 (clayt.nosp@m.onra.nosp@m.ycowe.nosp@m.n@gm.nosp@m.ail.c.nosp@m.om)

Date: 2024-02-04

{
    // Acquire a write lock on the waypoint vector.
    std::unique_lock<std::shared_mutex> lkWaypointListLock(m_muWaypointsMutex);
    // Delete any waypoint matching the given one from the list.
    m_vWaypointList.erase(std::remove(m_vWaypointList.begin(), m_vWaypointList.end(), stWaypoint), m_vWaypointList.end());
}

◆ DeleteWaypoint() [3/4]

void WaypointHandler::DeleteWaypoint ( const geoops::GPSCoordinate & stLocation )

Delete a waypoint from the WaypointHandler given a matching location. Any waypoint in the list that matches the given location will be removed.

Parameters

stLocation - The equivalent location that should be removed.

Author: clayjay3 (clayt.nosp@m.onra.nosp@m.ycowe.nosp@m.n@gm.nosp@m.ail.c.nosp@m.om)

Date: 2024-02-04

{
    // Acquire a write lock on the waypoint vector.
    std::unique_lock<std::shared_mutex> lkWaypointListLock(m_muWaypointsMutex);
    // Delete any waypoint matching the given location from the list.
    m_vWaypointList.erase(std::remove_if(m_vWaypointList.begin(),
                                         m_vWaypointList.end(),
                                         [stLocation](const geoops::Waypoint& stWaypoint) { return stWaypoint.GetGPSCoordinate() == stLocation; }),
                          m_vWaypointList.end());
}

◆ DeleteWaypoint() [4/4]

void WaypointHandler::DeleteWaypoint ( const geoops::UTMCoordinate & stLocation )

Delete a waypoint from the WaypointHandler given a matching location. Any waypoint in the list that matches the given location will be removed.

Parameters

stLocation - The equivalent location that should be removed.

Author: clayjay3 (clayt.nosp@m.onra.nosp@m.ycowe.nosp@m.n@gm.nosp@m.ail.c.nosp@m.om)

Date: 2024-02-04

{
    // Acquire a write lock on the waypoint vector.
    std::unique_lock<std::shared_mutex> lkWaypointListLock(m_muWaypointsMutex);
    // Delete any waypoint matching the given location from the list.
    m_vWaypointList.erase(std::remove_if(m_vWaypointList.begin(),
                                         m_vWaypointList.end(),
                                         [stLocation](const geoops::Waypoint& stWaypoint) { return stWaypoint.GetUTMCoordinate() == stLocation; }),
                          m_vWaypointList.end());
}

◆ DeletePath()

bool WaypointHandler::DeletePath ( const std::string & szPathName )

Delete the path vector stored at the given key.

Parameters

szPathName - The name/key of the path that was previously used to store the path.

Returns: true - Key was found and deleted successfully.; false - Key was not found and therefore nothing was deleted.

Author: clayjay3 (clayt.nosp@m.onra.nosp@m.ycowe.nosp@m.n@gm.nosp@m.ail.c.nosp@m.om)

Date: 2024-02-03

{
    // Acquire a write lock on the path unordered map.
    std::unique_lock<std::shared_mutex> lkPathsLock(m_muPathMutex);
    // Remove the given key and value from the map.
    return m_umStoredPaths.erase(szPathName);
}

◆ DeleteObstacle() [1/4]

void WaypointHandler::DeleteObstacle ( const long unsigned int nIndex )

Delete the obstacle at a given index from the waypoint handler obstacle list.

Parameters

nIndex - The index of the element to remove.

Author: clayjay3 (clayt.nosp@m.onra.nosp@m.ycowe.nosp@m.n@gm.nosp@m.ail.c.nosp@m.om)

Date: 2024-02-03

{
    // Acquire a read lock on the waypoint vector.
    std::shared_lock<std::shared_mutex> lkObjectListLock(m_muObstaclesMutex);
    // Check if the vector has at least one waypoint.
    if (nIndex < m_vPermanentObstacles.size())
    {
        // Release read lock.
        lkObjectListLock.unlock();
 
        // Acquire a write lock on the obstacle vector.
        std::unique_lock<std::shared_mutex> lkObjectListLock(m_muObstaclesMutex);
        // Delete the geoops::Waypoint at the index.
        m_vPermanentObstacles.erase(m_vPermanentObstacles.begin() + nIndex);
    }
    else
    {
        // Submit logger message.
        LOG_ERROR(logging::g_qSharedLogger,
                  "Attempted to delete an obstacle waypoint at index {} from the WaypointHandler but it is already empty or the index is out of bounds!",
                  nIndex);
    }
}

◆ DeleteObstacle() [2/4]

void WaypointHandler::DeleteObstacle ( const geoops::Waypoint & stWaypoint )

Delete an obstacle from the WaypointHandler given a matching location. Any waypoint in the list that matches the given location will be removed.

Parameters

stWaypoint - The equivalent obstacle location that should be removed.

Author: clayjay3 (clayt.nosp@m.onra.nosp@m.ycowe.nosp@m.n@gm.nosp@m.ail.c.nosp@m.om)

Date: 2024-02-04

{
    // Acquire a write lock on the obstacle vector.
    std::unique_lock<std::shared_mutex> lkObjectListLock(m_muObstaclesMutex);
    // Delete any waypoint matching the given one from the list.
    m_vPermanentObstacles.erase(std::remove(m_vPermanentObstacles.begin(), m_vPermanentObstacles.end(), stWaypoint), m_vPermanentObstacles.end());
}

◆ DeleteObstacle() [3/4]

void WaypointHandler::DeleteObstacle ( const geoops::GPSCoordinate & stLocation )

Delete an obstacle from the WaypointHandler given a matching location. Any waypoint in the list that matches the given location will be removed.

Parameters

stLocation - The equivalent obstacle location that should be removed.

Author: clayjay3 (clayt.nosp@m.onra.nosp@m.ycowe.nosp@m.n@gm.nosp@m.ail.c.nosp@m.om)

Date: 2024-02-04

{
    // Acquire a write lock on the obstacle vector.
    std::unique_lock<std::shared_mutex> lkObjectListLock(m_muObstaclesMutex);
    // Delete any waypoint matching the given location from the list.
    m_vPermanentObstacles.erase(std::remove_if(m_vPermanentObstacles.begin(),
                                               m_vPermanentObstacles.end(),
                                               [stLocation](const geoops::Waypoint& stWaypoint) { return stWaypoint.GetGPSCoordinate() == stLocation; }),
                                m_vPermanentObstacles.end());
}

◆ DeleteObstacle() [4/4]

void WaypointHandler::DeleteObstacle ( const geoops::UTMCoordinate & stLocation )

Delete an obstacle from the WaypointHandler given a matching location. Any waypoint in the list that matches the given location will be removed.

Parameters

stLocation - The equivalent obstacle location that should be removed.

Author: clayjay3 (clayt.nosp@m.onra.nosp@m.ycowe.nosp@m.n@gm.nosp@m.ail.c.nosp@m.om)

Date: 2024-02-04

{
    // Acquire a write lock on the obstacle vector.
    std::unique_lock<std::shared_mutex> lkObjectListLock(m_muObstaclesMutex);
    // Delete any waypoint matching the given location from the list.
    m_vPermanentObstacles.erase(std::remove_if(m_vPermanentObstacles.begin(),
                                               m_vPermanentObstacles.end(),
                                               [stLocation](const geoops::Waypoint& stWaypoint) { return stWaypoint.GetUTMCoordinate() == stLocation; }),
                                m_vPermanentObstacles.end());
}

◆ ClearWaypoints()

void WaypointHandler::ClearWaypoints ( )

Clears/deletes all Waypoints stored in the WaypointHandler.

Author: clayjay3 (clayt.nosp@m.onra.nosp@m.ycowe.nosp@m.n@gm.nosp@m.ail.c.nosp@m.om)

Date: 2024-02-03

{
    // Acquire a write lock on the waypoint vector.
    std::unique_lock<std::shared_mutex> lkWaypointListLock(m_muWaypointsMutex);
    // Clear the waypoint vector.
    m_vWaypointList.clear();
}

◆ ClearPaths()

void WaypointHandler::ClearPaths ( )

Clears/deletes all keys and paths store in the WaypointHandler.

Author: clayjay3 (clayt.nosp@m.onra.nosp@m.ycowe.nosp@m.n@gm.nosp@m.ail.c.nosp@m.om)

Date: 2024-02-03

{
    // Acquire a write lock on the path unordered map.
    std::unique_lock<std::shared_mutex> lkPathsLock(m_muPathMutex);
    // Clear the path map.
    m_umStoredPaths.clear();
}

◆ ClearObstacles()

void WaypointHandler::ClearObstacles ( )

Clears/deletes all permanent objects stored in the WaypointHandler.

Author: clayjay3 (clayt.nosp@m.onra.nosp@m.ycowe.nosp@m.n@gm.nosp@m.ail.c.nosp@m.om)

Date: 2024-02-03

{
    // Acquire a write lock on the path unordered map.
    std::unique_lock<std::shared_mutex> lkObstaclesLock(m_muObstaclesMutex);
    // Clear the obstacle vector.
    m_vPermanentObstacles.clear();
}

◆ PopNextWaypoint()

geoops::Waypoint WaypointHandler::PopNextWaypoint ( )

Removes and returns the next waypoint at the front of the list.

Returns: WaypointHandler::geoops::Waypoint - The next waypoint data stored in a geoops::Waypoint struct.

Author: clayjay3 (clayt.nosp@m.onra.nosp@m.ycowe.nosp@m.n@gm.nosp@m.ail.c.nosp@m.om)

Date: 2024-02-03

{
    // Acquire a read lock on the waypoint vector.
    std::shared_lock<std::shared_mutex> lkWaypointListLock(m_muWaypointsMutex);
    // Check if the vector has at least one waypoint.
    if (!m_vWaypointList.empty())
    {
        // Release read lock.
        lkWaypointListLock.unlock();
 
        // Acquire a write lock on the waypoint vector.
        std::unique_lock<std::shared_mutex> lkWaypointListLock(m_muWaypointsMutex);
        // Pop a waypoint from the front of the waypoint list and store it.
        geoops::Waypoint stWaypoint = m_vWaypointList[0];
        m_vWaypointList.erase(m_vWaypointList.begin());
        // Unlock shared mutex.
        lkWaypointListLock.unlock();
 
        // Return a copy of the waypoint.
        return stWaypoint;
    }
    else
    {
        // Submit logger message.
        LOG_ERROR(logging::g_qSharedLogger, "Attempted to pop a waypoint from the WaypointHandler but it is empty!");
 
        // Return an empty waypoint.
        return geoops::Waypoint(geoops::GPSCoordinate(), geoops::WaypointType::eUNKNOWN);
    }
}

Here is the caller graph for this function:

◆ PeekNextWaypoint()

const geoops::Waypoint WaypointHandler::PeekNextWaypoint ( )

Returns an immutable reference to the geoops::Waypoint struct at the front of the list without removing it.

Returns: const WaypointHandler::geoops::Waypoint - A reference to a geoops::Waypoint struct containing waypoint data.

Author: clayjay3 (clayt.nosp@m.onra.nosp@m.ycowe.nosp@m.n@gm.nosp@m.ail.c.nosp@m.om)

Date: 2024-02-03

{
    // Acquire a read lock on the waypoint vector.
    std::shared_lock<std::shared_mutex> lkWaypointListLock(m_muWaypointsMutex);
    // Check if the vector has at least one waypoint.
    if (!m_vWaypointList.empty())
    {
        // Return an immutable reference to the waypoint.
        return m_vWaypointList.front();
    }
    else
    {
        // Submit logger message.
        LOG_ERROR(logging::g_qSharedLogger, "Attempted to peek a waypoint from the WaypointHandler but it is empty!");
 
        // Return an empty waypoint.
        return geoops::Waypoint(geoops::GPSCoordinate(), geoops::WaypointType::eUNKNOWN);
    }
}

Here is the caller graph for this function:

◆ RetrieveWaypointAtIndex()

const geoops::Waypoint WaypointHandler::RetrieveWaypointAtIndex ( const long unsigned int nIndex )

Retrieve an immutable reference to the waypoint at the given index.

Parameters

nIndex - The index of the element to retrieve.

Returns: const WaypointHandler::geoops::Waypoint - An immutable reference to the geoops::Waypoint containing data.

Author: clayjay3 (clayt.nosp@m.onra.nosp@m.ycowe.nosp@m.n@gm.nosp@m.ail.c.nosp@m.om)

Date: 2024-02-03

{
    // Acquire a read lock on the waypoint vector.
    std::shared_lock<std::shared_mutex> lkWaypointListLock(m_muWaypointsMutex);
    // Check if the vector has at least one waypoint.
    if (nIndex < m_vWaypointList.size())
    {
        // Return an immutable reference to the waypoint at the index.
        return m_vWaypointList[nIndex];
    }
    else
    {
        // Submit logger message.
        LOG_ERROR(logging::g_qSharedLogger,
                  "Attempted to retrieve a waypoint at index {} from the WaypointHandler but it is empty or the index is out of bounds!",
                  nIndex);
 
        // Return an empty waypoint.
        return geoops::Waypoint(geoops::GPSCoordinate(), geoops::WaypointType::eUNKNOWN);
    }
}

◆ RetrievePath()

const std::vector< geoops::Waypoint > WaypointHandler::RetrievePath ( const std::string & szPathName )

Retrieve an immutable reference to the path at the given path name/key.

Parameters

szPathName - The name/key of the path that was previously used to store the path.

Returns: const std::vector<WaypointHandler::geoops::Waypoint> - A reference to the geoops::Waypoint vector located at the given key.

Author: clayjay3 (clayt.nosp@m.onra.nosp@m.ycowe.nosp@m.n@gm.nosp@m.ail.c.nosp@m.om)

Date: 2024-02-03

{
    // Acquire a read lock on the path unordered map.
    std::shared_lock<std::shared_mutex> lkPathsLock(m_muPathMutex);
    // Check if the map contains the given key.
    if (m_umStoredPaths.count(szPathName) > 0)
    {
        // Return the path vector at the given key.
        return m_umStoredPaths[szPathName];
    }
    else
    {
        // Return an empty vector.
        return std::vector<geoops::Waypoint>();
    }
}

◆ RetrieveObstacleAtIndex()

const geoops::Waypoint WaypointHandler::RetrieveObstacleAtIndex ( const long unsigned int nIndex )

Retrieve an immutable reference to the obstacle at the given index.

Parameters

nIndex - The index of the element to retrieve.

Returns: const WaypointHandler::geoops::Waypoint - An immutable reference to the obstacle geoops::Waypoint containing data.

Author: clayjay3 (clayt.nosp@m.onra.nosp@m.ycowe.nosp@m.n@gm.nosp@m.ail.c.nosp@m.om)

Date: 2024-02-03

{
    // Acquire a read lock on the waypoint vector.
    std::shared_lock<std::shared_mutex> lkObstaclesLock(m_muObstaclesMutex);
    // Check if the vector has at least one waypoint.
    if (nIndex < m_vPermanentObstacles.size())
    {
        // Return an immutable reference to the waypoint at the index.
        return m_vPermanentObstacles[nIndex];
    }
    else
    {
        // Submit logger message.
        LOG_ERROR(logging::g_qSharedLogger,
                  "Attempted to retrieve a obstacle at index {} from the WaypointHandler but it is empty or the index is out of bounds!",
                  nIndex);
 
        // Return an empty waypoint.
        return geoops::Waypoint(geoops::GPSCoordinate(), geoops::WaypointType::eUNKNOWN);
    }
}

◆ GetAllWaypoints()

const std::vector< geoops::Waypoint > WaypointHandler::GetAllWaypoints ( )

Accessor for the full list of current waypoints stored in the WaypointHandler.

Returns: const std::vector<WaypointHandler::geoops::Waypoint> - A vector of geoops::Waypoint structs currently stored in the WaypointHandler.

Author: clayjay3 (clayt.nosp@m.onra.nosp@m.ycowe.nosp@m.n@gm.nosp@m.ail.c.nosp@m.om)

Date: 2024-02-04

{
    // Acquire a read lock on the waypoint vector.
    std::shared_lock<std::shared_mutex> lkWaypointListLock(m_muWaypointsMutex);
    // Return a copy of the current waypoint list.
    return m_vWaypointList;
}

◆ GetAllObstacles()

const std::vector< geoops::Waypoint > WaypointHandler::GetAllObstacles ( )

Accessor for the full list of current obstacle stored in the WaypointHandler.

Returns: const std::vector<WaypointHandler::geoops::Waypoint> - A vector of geoops::Waypoint structs representing objects that are currently stored in the WaypointHandler.

Author: clayjay3 (clayt.nosp@m.onra.nosp@m.ycowe.nosp@m.n@gm.nosp@m.ail.c.nosp@m.om)

Date: 2024-02-04

{
    // Acquire a read lock on the path unordered map.
    std::shared_lock<std::shared_mutex> lkObstaclesLock(m_muObstaclesMutex);
    // Return a copy of the current obstacle list.
    return m_vPermanentObstacles;
}

Here is the caller graph for this function:

◆ GetWaypointCount()

int WaypointHandler::GetWaypointCount ( )

Accessor for the number of elements on the WaypointHandler's waypoint vector.

Returns: int - The size of the waypoint vector.

Author: clayjay3 (clayt.nosp@m.onra.nosp@m.ycowe.nosp@m.n@gm.nosp@m.ail.c.nosp@m.om)

Date: 2024-02-03

{
    // Acquire a read lock on the waypoint vector.
    std::shared_lock<std::shared_mutex> lkWaypointListLock(m_muWaypointsMutex);
    // Return total number of waypoints stored.
    return m_vWaypointList.size();
}

◆ GetPathsCount()

int WaypointHandler::GetPathsCount ( )

Accessor for the number of paths stored in the WaypointHandler.

Returns: int - The size of the unordered_map storing the paths.

Author: clayjay3 (clayt.nosp@m.onra.nosp@m.ycowe.nosp@m.n@gm.nosp@m.ail.c.nosp@m.om)

Date: 2024-02-03

{
    // Acquire a read lock on the path unordered map.
    std::shared_lock<std::shared_mutex> lkPathsLock(m_muPathMutex);
    // Return total number of paths.
    return m_umStoredPaths.size();
}

◆ GetObstaclesCount()

int WaypointHandler::GetObstaclesCount ( )

Accessor for the number of elements on the WaypointHandler's obstacle vector.

Returns: int - The size of the obstacle vector.

Author: clayjay3 (clayt.nosp@m.onra.nosp@m.ycowe.nosp@m.n@gm.nosp@m.ail.c.nosp@m.om)

Date: 2024-02-03

{
    // Acquire a write lock on the waypoint vector.
    std::shared_lock<std::shared_mutex> lkObstaclesLock(m_muObstaclesMutex);
    // Return total number of objects stored.
    return m_vPermanentObstacles.size();
}

◆ SmartRetrieveRoverPose()

geoops::RoverPose WaypointHandler::SmartRetrieveRoverPose	(	bool	bVIOHeading = `true`,
		bool	bVIOTracking = `false`
	)

Retrieve the rover's current position and heading. Automatically picks between getting the position/heading from the NavBoard, ZEDSDK Fusion module, or ZED positional tracking. In most cases, this will be the method that should be called over getting the data directly from NavBoard.

Parameters

bVIOHeading	- Whether to use ZED Heading Fusion.
bVIOTracking	- Whether to use ZED Positional Tracking.

Returns: geoops::RoverPose - The current position and heading (pose) of the rover stored in a RoverPose struct.

Author: clayjay3 (clayt.nosp@m.onra.nosp@m.ycowe.nosp@m.n@gm.nosp@m.ail.c.nosp@m.om)

Date: 2024-04-06

{
    // Get and store the normal GPS position and heading from NavBoard.
    geoops::GPSCoordinate stCurrentGPSPosition = globals::g_pNavigationBoard->GetGPSData();
    double dCurrentGPSHeading                  = globals::g_pNavigationBoard->GetHeading();
 
    // Create instance variables.
    std::shared_ptr<ZEDCamera> pMainCam        = globals::g_pCameraHandler->GetZED(CameraHandler::ZEDCamName::eHeadMainCam);
    geoops::GPSCoordinate stCurrentVIOPosition = stCurrentGPSPosition;
    double dCurrentHeading                     = dCurrentGPSHeading;
    bool bVIOGPSFused                          = false;
    static bool bAlreadyPrinted                = false;
 
    if ((bVIOHeading || bVIOTracking) && !constants::MODE_SIM)
    {
        // Check if the main ZED camera is opened and the fusion module is initialized.
        if (pMainCam->GetCameraIsOpen() && pMainCam->GetPositionalTrackingEnabled())
        {
            // Get the fused positional tracking state of the main camera.
            sl::GNSS_FUSION_STATUS slGNSSFusionStatus = pMainCam->GetFusedPositionalTrackingState().gnss_fusion_status;
 
            // Check if GNSS fusion is enabled and current GPS data from has differential accuracy and GNSS fusion is calibrated.
            if (constants::FUSION_ENABLE_GNSS_FUSION && pMainCam->GetIsFusionMaster() && stCurrentGPSPosition.bIsDifferential &&
                (slGNSSFusionStatus == sl::GNSS_FUSION_STATUS::OK || slGNSSFusionStatus == sl::GNSS_FUSION_STATUS::RECALIBRATION_IN_PROGRESS))
            {
                // Create instance variables.
                sl::GeoPose slCurrentCameraGeoPose;
                ZEDCam::Pose stCurrentCameraVIOPose;
 
                // Check if position VIO tracking should be used.
                if (bVIOTracking)
                {
                    // Get the current camera pose from the ZEDCam.
                    std::future<bool> fuResultStatus = pMainCam->RequestFusionGeoPoseCopy(slCurrentCameraGeoPose);
                    if (fuResultStatus.get())
                    {
                        // Repack the camera pose into a GPSCoordinate.
                        stCurrentVIOPosition.dLatitude  = slCurrentCameraGeoPose.latlng_coordinates.getLatitude(false);
                        stCurrentVIOPosition.dLongitude = slCurrentCameraGeoPose.latlng_coordinates.getLongitude(false);
                        stCurrentVIOPosition.dAltitude  = slCurrentCameraGeoPose.latlng_coordinates.getAltitude();
 
                        // Set fused toggle.
                        bVIOGPSFused = true;
                    }
                }
 
                // Check if heading VIO tracking should be used.
                if (bVIOHeading)
                {
                    // Get the current camera pose from the ZEDCam.
                    std::future<bool> fuResultStatus2 = pMainCam->RequestPositionalPoseCopy(stCurrentCameraVIOPose);
                    if (fuResultStatus2.get())
                    {
                        // Repack the camera pose into a UTMCoordinate.
                        // dCurrentHeading = slCurrentCameraGeoPose.heading * (180.0 / M_PI);    // This doesn't work because the heading is on the wrong axis for some
                        // reason.
                        dCurrentHeading = stCurrentCameraVIOPose.stEulerAngles.dYO;
 
                        // Set fused toggle.
                        bVIOGPSFused = true;
                    }
                }
 
                // Check toggle so we only print once.
                if (bAlreadyPrinted)
                {
                    // Submit logger message.
                    LOG_NOTICE(logging::g_qSharedLogger, "GNSS Fusion has now converged! Using GNSS Fusion for rover pose...");
                    // Set toggle.
                    bAlreadyPrinted = false;
                }
            }
            else
            {
                // Create instance variables.
                ZEDCam::Pose stCurrentCameraVIOPose;
 
                // Get the current camera pose from the ZEDCam.
                std::future<bool> fuResultStatus = pMainCam->RequestPositionalPoseCopy(stCurrentCameraVIOPose);
                // Wait for future to be fulfilled.
                if (fuResultStatus.get())
                {
                    // Check if position VIO tracking should be used.
                    if (bVIOTracking)
                    {
                        // Camera is using UTM. Modify current GPS position to be camera's position.
                        geoops::UTMCoordinate stCameraUTMLocation = geoops::ConvertGPSToUTM(stCurrentGPSPosition);
                        // Repack the camera pose into a GPSCoordinate.
                        stCameraUTMLocation.dEasting  = stCurrentCameraVIOPose.stTranslation.dX;
                        stCameraUTMLocation.dNorthing = stCurrentCameraVIOPose.stTranslation.dZ;
                        stCameraUTMLocation.dAltitude = stCurrentCameraVIOPose.stTranslation.dY;
                        // Convert back to GPS coordinate and store.
                        stCurrentVIOPosition = geoops::ConvertUTMToGPS(stCameraUTMLocation);
                    }
 
                    // Check if heading VIO tracking should be used.
                    if (bVIOHeading)
                    {
                        // Get compass heading based off of the ZED's aligned accelerometer.
                        dCurrentHeading = stCurrentCameraVIOPose.stEulerAngles.dYO;
                    }
 
                    // Set fused toggle.
                    bVIOGPSFused = false;
                }
 
                // Check toggle so we only print once.
                if (!bAlreadyPrinted && constants::FUSION_ENABLE_GNSS_FUSION)
                {
                    // Submit logger message.
                    LOG_NOTICE(logging::g_qSharedLogger, "GNSS Fusion is still calibrating. Using VIO tracking for rover pose...");
                    // Set toggle.
                    bAlreadyPrinted = true;
                }
            }
        }
        else
        {
            LOG_WARNING_LIMIT(std::chrono::seconds(5),
                              logging::g_qSharedLogger,
                              "Positional tracking is not enabled or camera is not open! Using NavBoard GPS data for rover pose...");
        }
    }
 
    // Submit a debug print for the current rover pose.
    geoops::UTMCoordinate stCurrentUTMPosition = geoops::ConvertGPSToUTM(stCurrentVIOPosition);
    LOG_DEBUG(logging::g_qSharedLogger,
              "Rover Pose is currently: {} (easting), {} (northing), {} (alt), {} (degrees), GNSS/VIO FUSED? = {}, VIOPosition = {}, VIOHeading = {}",
              stCurrentUTMPosition.dEasting,
              stCurrentUTMPosition.dNorthing,
              stCurrentUTMPosition.dAltitude,
              dCurrentHeading,
              bVIOGPSFused ? "true" : "false",
              bVIOTracking ? "true" : "false",
              bVIOHeading ? "true" : "false");
 
    // Submit a debug print for some error metrics pertaining to the ZED camera and NavBoard locations and headings.
    double dHeadingError  = dCurrentHeading - dCurrentGPSHeading;
    double dEastingError  = ConvertGPSToUTM(stCurrentGPSPosition).dEasting - stCurrentUTMPosition.dEasting;
    double dNorthingError = ConvertGPSToUTM(stCurrentGPSPosition).dNorthing - stCurrentUTMPosition.dNorthing;
 
    // Check if VIO tracking or heading is being used.
    if (bVIOHeading || 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 = "--------[ 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" + "Heading Error:\n" + std::to_string(dCurrentGPSHeading) + " (NavBoard) vs. " + std::to_string(dCurrentHeading) +
                                     " (Camera) = " + std::to_string(dHeadingError) + " (error)\n GNSS/VIO FUSED? = " + (bVIOGPSFused ? "true" : "false") +
                                     ", VIOPosition = " + (bVIOTracking ? "true" : "false") + ", VIOHeading = " + (bVIOHeading ? "true" : "false");
        // Submit the error metrics to the logger.
        LOG_DEBUG(logging::g_qSharedLogger, "{}", szErrorMetrics);
    }
 
    return geoops::RoverPose(stCurrentVIOPosition, dCurrentHeading);
}

Here is the call graph for this function:

Here is the caller graph for this function:

◆ SmartRetrieveVelocity()

double WaypointHandler::SmartRetrieveVelocity ( )

Retrieve the rover's current velocity. Currently there is no easy way to get the velocity of the ZEDCam so this method just returns the GPS-based velocity.

Returns: double - The current velocity of the rover. (m/s)

Author: clayjay3 (clayt.nosp@m.onra.nosp@m.ycowe.nosp@m.n@gm.nosp@m.ail.c.nosp@m.om)

Date: 2024-04-06

{
    // Return the GPS-based velocity from the NavBoard.
    return globals::g_pNavigationBoard->GetVelocity();
}

Here is the call graph for this function:

Here is the caller graph for this function:

◆ SmartRetrieveAngularVelocity()

double WaypointHandler::SmartRetrieveAngularVelocity ( )

Retrieve the rover's current velocity. Currently there is no easy way to get the velocity of the ZEDCam so this method just returns the GPS-based velocity.

Returns: double - The current angular velocity of the rover. (deg/s)

Author: clayjay3 (clayt.nosp@m.onra.nosp@m.ycowe.nosp@m.n@gm.nosp@m.ail.c.nosp@m.om)

Date: 2024-04-06

{
    // Return the GPS-based angular velocity from the NavBoard.
    return globals::g_pNavigationBoard->GetAngularVelocity();
}

Here is the call graph for this function:

Here is the caller graph for this function:

Member Data Documentation

◆ AddPositionLegCallback

const std::function<void(const rovecomm::RoveCommPacket<double>&, const sockaddr_in&)> WaypointHandler::AddPositionLegCallback

private

Initial value:

=
            [this](const rovecomm::RoveCommPacket<double>& stPacket, const sockaddr_in& stdAddr)
        {
            
            (void) stdAddr;
 
            
            geoops::Waypoint stNavWaypoint(geoops::GPSCoordinate(stPacket.vData[0], stPacket.vData[1]),
                                           geoops::WaypointType::eNavigationWaypoint,
                                           0.0,
                                           stPacket.vData[2]);
 
            
            std::unique_lock<std::shared_mutex> lkWaypointsLock(m_muWaypointsMutex);
            
            m_vWaypointList.emplace_back(stNavWaypoint);
            
            lkWaypointsLock.unlock();
 
            
            LOG_NOTICE(logging::g_qSharedLogger,
                       "Incoming Navigation Waypoint Data: Added (lat: {}, lon: {}, id: {}) to WaypointHandler queue.",
                       stPacket.vData[0],
                       stPacket.vData[1],
                       stPacket.vData[2]);
        }

Callback function that is called whenever RoveComm receives new ADDPOSITIONLEG packet.

Author: clayjay3 (clayt.nosp@m.onra.nosp@m.ycowe.nosp@m.n@gm.nosp@m.ail.c.nosp@m.om)

Date: 2024-03-03

        {
            // Not using this.
            (void) stdAddr;
 
            // Create new waypoint struct with data from the RoveComm packet.
            geoops::Waypoint stNavWaypoint(geoops::GPSCoordinate(stPacket.vData[0], stPacket.vData[1]),
                                           geoops::WaypointType::eNavigationWaypoint,
                                           0.0,
                                           stPacket.vData[2]);
 
            // Acquire write lock for writing to waypoints vector.
            std::unique_lock<std::shared_mutex> lkWaypointsLock(m_muWaypointsMutex);
            // Queue waypoint.
            m_vWaypointList.emplace_back(stNavWaypoint);
            // Unlock mutex.
            lkWaypointsLock.unlock();
 
            // Submit logger message.
            LOG_NOTICE(logging::g_qSharedLogger,
                       "Incoming Navigation Waypoint Data: Added (lat: {}, lon: {}, id: {}) to WaypointHandler queue.",
                       stPacket.vData[0],
                       stPacket.vData[1],
                       stPacket.vData[2]);
        };

◆ AddMarkerLegCallback

const std::function<void(const rovecomm::RoveCommPacket<double>&, const sockaddr_in&)> WaypointHandler::AddMarkerLegCallback

private

Callback function that is called whenever RoveComm receives new ADDMARKERLEG packet.

Author: clayjay3 (clayt.nosp@m.onra.nosp@m.ycowe.nosp@m.n@gm.nosp@m.ail.c.nosp@m.om)

Date: 2024-03-03

        {
            // Not using this.
            (void) stdAddr;
 
            // Create instance variables.
            int nMarkerID  = stPacket.vData[2];
            double dRadius = stPacket.vData[3];
 
            // Limit the radius to 0-40.
            if (dRadius < 0)
            {
                // Submit logger message.
                LOG_WARNING(logging::g_qSharedLogger, "Incoming Marker Waypoint Data: Radius is less than 0, setting to 0.");
                dRadius = 0;
            }
            else if (dRadius > 40)
            {
                // Submit logger message.
                LOG_WARNING(logging::g_qSharedLogger, "Incoming Marker Waypoint Data: Radius is greater than 40, setting to 40.");
                dRadius = 40;
            }
 
            // Create new waypoint struct with data from the RoveComm packet.
            geoops::Waypoint stMarkerWaypoint(geoops::GPSCoordinate(stPacket.vData[0], stPacket.vData[1]), geoops::WaypointType::eTagWaypoint, dRadius, nMarkerID);
 
            // Acquire write lock for writing to waypoints vector.
            std::unique_lock<std::shared_mutex> lkWaypointsLock(m_muWaypointsMutex);
            // Queue waypoint.
            m_vWaypointList.emplace_back(stMarkerWaypoint);
            // Unlock mutex.
            lkWaypointsLock.unlock();
 
            // Submit logger message.
            LOG_NOTICE(logging::g_qSharedLogger,
                       "Incoming Marker Waypoint Data: Added (lat: {}, lon: {}, marker ID: {}, radius: {}) to WaypointHandler queue.",
                       stPacket.vData[0],
                       stPacket.vData[1],
                       nMarkerID,
                       dRadius);
        };

◆ AddObjectLegCallback

const std::function<void(const rovecomm::RoveCommPacket<double>&, const sockaddr_in&)> WaypointHandler::AddObjectLegCallback

private

Callback function that is called whenever RoveComm receives new ADDOBJECTLEG packet.

Author: clayjay3 (clayt.nosp@m.onra.nosp@m.ycowe.nosp@m.n@gm.nosp@m.ail.c.nosp@m.om)

Date: 2024-03-03

        {
            // Not using this.
            (void) stdAddr;
 
            // Create instance variables.
            geoops::WaypointType eWaypointType = geoops::WaypointType::eObjectWaypoint;
            double dObjectID                   = stPacket.vData[2];
            double dRadius                     = stPacket.vData[3];
 
            // Parse the object ID from the RoveComm packet to a waypoint type.
            if (dObjectID == static_cast<int>(manifest::Autonomy::AUTONOMYWAYPOINTTYPES::MALLET))
            {
                eWaypointType = geoops::WaypointType::eMalletWaypoint;
            }
            else if (dObjectID == static_cast<int>(manifest::Autonomy::AUTONOMYWAYPOINTTYPES::WATERBOTTLE))
            {
                eWaypointType = geoops::WaypointType::eWaterBottleWaypoint;
            }
            else
            {
                eWaypointType = geoops::WaypointType::eObjectWaypoint;
            }
 
            // Limit the radius to 0-40.
            if (dRadius < 0)
            {
                // Submit logger message.
                LOG_WARNING(logging::g_qSharedLogger, "Incoming Object Waypoint Data: Radius is less than 0, setting to 0.");
                dRadius = 0;
            }
            else if (dRadius > 40)
            {
                // Submit logger message.
                LOG_WARNING(logging::g_qSharedLogger, "Incoming Object Waypoint Data: Radius is greater than 40, setting to 40.");
                dRadius = 40;
            }
 
            // Create new waypoint struct with data from the RoveComm packet.
            geoops::Waypoint stObjectWaypoint(geoops::GPSCoordinate(stPacket.vData[0], stPacket.vData[1]), eWaypointType, dRadius);
 
            // Acquire write lock for writing to waypoints vector.
            std::unique_lock<std::shared_mutex> lkWaypointsLock(m_muWaypointsMutex);
            // Queue waypoint.
            m_vWaypointList.emplace_back(stObjectWaypoint);
            // Unlock mutex.
            lkWaypointsLock.unlock();
 
            // Submit logger message.
            LOG_NOTICE(logging::g_qSharedLogger,
                       "Incoming Object Waypoint Data: Added (lat: {}, lon: {}, id: {}, radius: {}) to WaypointHandler queue.",
                       stPacket.vData[0],
                       stPacket.vData[1],
                       dObjectID,
                       dRadius);
        };

◆ AddObstacleCallback

const std::function<void(const rovecomm::RoveCommPacket<double>&, const sockaddr_in&)> WaypointHandler::AddObstacleCallback

private

Callback function that is called whenever RoveComm receives new ADDOBSTACLE packet.

Author: clayjay3 (clayt.nosp@m.onra.nosp@m.ycowe.nosp@m.n@gm.nosp@m.ail.c.nosp@m.om)

Date: 2025-01-06

        {
            // Not using this.
            (void) stdAddr;
 
            // Create instance variables.
            double dRadius = stPacket.vData[2];
 
            // Limit the radius to 0-40.
            if (dRadius < 0)
            {
                // Submit logger message.
                LOG_WARNING(logging::g_qSharedLogger, "Incoming Obstacle Waypoint Data: Radius is less than 0, setting to 0.");
                dRadius = 0;
            }
            else if (dRadius > 40)
            {
                // Submit logger message.
                LOG_WARNING(logging::g_qSharedLogger, "Incoming Obstacle Waypoint Data: Radius is greater than 40, setting to 40.");
                dRadius = 40;
            }
 
            // Create new waypoint struct with data from the RoveComm packet.
            geoops::Waypoint stObstacleWaypoint(geoops::GPSCoordinate(stPacket.vData[0], stPacket.vData[1]), geoops::WaypointType::eObstacleWaypoint, dRadius);
 
            // Acquire write lock for writing to waypoints vector.
            std::unique_lock<std::shared_mutex> lkWaypointsLock(m_muWaypointsMutex);
            // Queue waypoint.
            m_vPermanentObstacles.emplace_back(stObstacleWaypoint);
            // Unlock mutex.
            lkWaypointsLock.unlock();
 
            // Submit logger message.
            LOG_NOTICE(logging::g_qSharedLogger,
                       "Incoming Obstacle Waypoint Data: Added (lat: {}, lon: {}, radius: {}) to WaypointHandler queue. Total Obstacles: {}",
                       stPacket.vData[0],
                       stPacket.vData[1],
                       dRadius,
                       m_vPermanentObstacles.size());
        };

◆ ClearWaypointsCallback

const std::function<void(const rovecomm::RoveCommPacket<uint8_t>&, const sockaddr_in&)> WaypointHandler::ClearWaypointsCallback

private

Initial value:

=
            [this](const rovecomm::RoveCommPacket<uint8_t>& stPacket, const sockaddr_in& stdAddr)
        {
            
            (void) stPacket;
            (void) stdAddr;
 
            std::unique_lock<std::shared_mutex> lkWaypointsLock(m_muWaypointsMutex);
            
            m_vWaypointList.clear();
            
            lkWaypointsLock.unlock();
 
            LOG_NOTICE(logging::g_qSharedLogger, "Incoming Clear Waypoints packet: Cleared WaypointHandler queue.");
        }

Callback function that is called whenever RoveComm receives new CLEARWAYPOINTS packet.

Author: clayjay3 (clayt.nosp@m.onra.nosp@m.ycowe.nosp@m.n@gm.nosp@m.ail.c.nosp@m.om)

Date: 2024-03-03

        {
            // Not using this.
            (void) stPacket;
            (void) stdAddr;
 
            // Acquire write lock for writing to waypoints vector.
            std::unique_lock<std::shared_mutex> lkWaypointsLock(m_muWaypointsMutex);
            // Clear waypoints queue.
            m_vWaypointList.clear();
            // Unlock mutex.
            lkWaypointsLock.unlock();
 
            // Submit logger message.
            LOG_NOTICE(logging::g_qSharedLogger, "Incoming Clear Waypoints packet: Cleared WaypointHandler queue.");
        };

◆ ClearObstaclesCallback

const std::function<void(const rovecomm::RoveCommPacket<uint8_t>&, const sockaddr_in&)> WaypointHandler::ClearObstaclesCallback

private

Initial value:

=
            [this](const rovecomm::RoveCommPacket<uint8_t>& stPacket, const sockaddr_in& stdAddr)
        {
            
            (void) stPacket;
            (void) stdAddr;
 
            std::unique_lock<std::shared_mutex> lkObstaclesLock(m_muObstaclesMutex);
            
            m_vPermanentObstacles.clear();
            
            lkObstaclesLock.unlock();
 
            LOG_NOTICE(logging::g_qSharedLogger, "Incoming Clear Obstacles packet: Cleared permanent obstacles list.");
        }

Callback function that is called whenever RoveComm receives new CLEAROBSTACLES packet.

Author: clayjay3 (clayt.nosp@m.onra.nosp@m.ycowe.nosp@m.n@gm.nosp@m.ail.c.nosp@m.om)

Date: 2025-02-19

        {
            // Not using this.
            (void) stPacket;
            (void) stdAddr;
 
            // Acquire write lock for obstacle vector.
            std::unique_lock<std::shared_mutex> lkObstaclesLock(m_muObstaclesMutex);
            // Clear obstacles queue.
            m_vPermanentObstacles.clear();
            // Unlock mutex.
            lkObstaclesLock.unlock();
 
            // Submit logger message.
            LOG_NOTICE(logging::g_qSharedLogger, "Incoming Clear Obstacles packet: Cleared permanent obstacles list.");
        };

The documentation for this class was generated from the following files:

src/handlers/WaypointHandler.h
src/handlers/WaypointHandler.cpp

Public Member Functions

Private Attributes

Detailed Description

Constructor & Destructor Documentation

◆ WaypointHandler()

◆ ~WaypointHandler()

Member Function Documentation

◆ AddWaypoint() [1/3]

◆ AddWaypoint() [2/3]

◆ AddWaypoint() [3/3]

◆ StorePath() [1/3]

◆ StorePath() [2/3]

◆ StorePath() [3/3]

◆ AddObstacle() [1/3]

◆ AddObstacle() [2/3]

◆ AddObstacle() [3/3]

◆ DeleteWaypoint() [1/4]

◆ DeleteWaypoint() [2/4]

◆ DeleteWaypoint() [3/4]

◆ DeleteWaypoint() [4/4]

◆ DeletePath()

◆ DeleteObstacle() [1/4]

◆ DeleteObstacle() [2/4]

◆ DeleteObstacle() [3/4]

◆ DeleteObstacle() [4/4]

◆ ClearWaypoints()

◆ ClearPaths()

◆ ClearObstacles()

◆ PopNextWaypoint()

◆ PeekNextWaypoint()

◆ RetrieveWaypointAtIndex()

◆ RetrievePath()

◆ RetrieveObstacleAtIndex()

◆ GetAllWaypoints()

◆ GetAllObstacles()

◆ GetWaypointCount()

◆ GetPathsCount()

◆ GetObstaclesCount()

◆ SmartRetrieveRoverPose()

◆ SmartRetrieveVelocity()

◆ SmartRetrieveAngularVelocity()

Member Data Documentation

◆ AddPositionLegCallback

◆ AddMarkerLegCallback

◆ AddObjectLegCallback

◆ AddObstacleCallback

◆ ClearWaypointsCallback

◆ ClearObstaclesCallback