Trajectory Management object.

#include <TrajManager.hpp>

Public Member Functions
	TrajManager (std::string callsign, std::string config)
	Construct a new Traj Manager object. More...

void	ReadParamFromFile (std::string config)
	Read parameters from file. More...

void	UpdateDubinsPlannerParameters (DubinsParams_t &params)
	Update dubins planner parameters. More...

int	FindPath (std::string planID, larcfm::Position fromPosition, larcfm::Position toPosition, larcfm::Velocity fromVelocity, larcfm::Velocity toVelocity)
	Compute path. More...

int	GetTotalWaypoints (std::string planID)
	Get the Total Waypoints. More...

int	GetWaypoint (std::string planID, int wpid, waypoint_t &wp)
	Get the Waypoint object. More...

larcfm::Plan *	GetPlan (std::string planID)
	Get pointer to Plan object. More...

void	ClearAllPlans ()
	Clear all plans.

void	ClearFences ()
	Clear fences.

void	InputGeofenceData (int type, int index, int totalVertices, double floor, double ceiling, double pos[][2])
	Input geofence. More...

void	InputGeofenceData (int type, int index, int totalVertices, double floor, double ceiling, std::vector< larcfm::Position > &vertices)
	Input geofence. More...

void	InputFlightPlan (const std::string &plan_id, const std::list< waypoint_t > &waypoints, const double initHeading, bool repair, double repairTurnRate)
	Input a flightplan. More...

int	InputTraffic (std::string callsign, larcfm::Position &position, larcfm::Velocity &velocity, double time)
	Input traffic data. More...

std::string	PlanToString (std::string planID)
	Convert plan to string. More...

void	StringToPlan (std::string planID, std::string inputString)
	Convert string to plan. More...

void	CombinePlan (std::string planA, std::string planB, int index)
	Combine planA with planB from given index. More...

void	SetPlanOffset (std::string planID, int n, double offset)
	Set the time Offset for given plan. More...

larcfm::Position	GetPlanPosition (std::string planID, double t)
	Get the position on plan at given t. More...

trajectoryMonitorData_t	MonitorTrajectory (double time, std::string planID, larcfm::Position pos, larcfm::Velocity vel, int nextWP1, int nextWP2)
	Monitor trajectory. More...

Public Attributes
larcfm::Position	startPos
	start position for planning

larcfm::Position	endPos
	goal position for planning

larcfm::Velocity	startVel
	start velocity for planning

larcfm::Velocity	endVel
	goal velocity for planning

Private Member Functions
int64_t	FindDubinsPath (std::string planID)
	Compute new path. More...

double	FindTimeToFenceViolation (larcfm::Poly3D polygon, larcfm::Vect3 so, larcfm::Velocity vel)
	Function to compute projected time to violation to all fence edges. More...

std::vector< double >	ComputePlanOffsets (const std::string planID, int nextWP, larcfm::Position currentPos, larcfm::Velocity currentVel, double timeAtPos)
	Compute cross track (perpendicular deviation), along track (longitudinal deviations) and time delays. More...

bool	CheckLineOfSightconflict (larcfm::Position start, larcfm::Position end)
	Check line of sight conflict. More...

void	LogInput ()
	Log inputs to planning.

Private Attributes
DubinsPlanner	dbPlanner
	Dubins planner object.

double	wellClearDistH
	well clear radius for planning

double	wellClearDistV
	well clear height for planning

int	numPlans
	number of plans

std::ofstream	log
	log holder

std::list< larcfm::Plan >	flightPlans
	list of plans

std::list< larcfm::Plan >	trafficPlans
	list of plans for traffic vehicles (intent information)

std::list< fenceObject >	fenceList
	list of fences

std::map< std::string, pObject >	trafficList
	list of traffic

Constructor & Destructor Documentation

◆ TrajManager()

TrajManager::TrajManager	(	std::string	callsign,
		std::string	config
	)

Parameters

callsign	callsign string
config	configuration file

Member Function Documentation

◆ CheckLineOfSightconflict()

bool TrajManager::CheckLineOfSightconflict	(	larcfm::Position	start,
		larcfm::Position	end
	)

private

Parameters

start	start position
end	end position

◆ CombinePlan()

void TrajManager::CombinePlan	(	std::string	planA,
		std::string	planB,
		int	index
	)

Parameters

planA
planB
index

◆ ComputePlanOffsets()

std::vector< double > TrajManager::ComputePlanOffsets	(	const std::string	planID,
		int	nextWP,
		larcfm::Position	currentPos,
		larcfm::Velocity	currentVel,
		double	timeAtPos
	)

private

Parameters

planID	id of plan to compute metrics
nextWP	next waypoint index
currentPos	current position
currentVel	current velocity
timeAtPos	current time

Returns: std::vector<double> cross track offset, longitudinal offset, time offset

◆ FindDubinsPath()

int64_t TrajManager::FindDubinsPath ( std::string planID )

private

Parameters

planID id to assign to plan

Returns: int64_t total number of waypoints computed

◆ FindPath()

int TrajManager::FindPath	(	std::string	planID,
		larcfm::Position	fromPosition,
		larcfm::Position	toPosition,
		larcfm::Velocity	fromVelocity,
		larcfm::Velocity	toVelocity
	)

Parameters

planID	plan id to assign to this plan
fromPosition	start position
toPosition	end position
fromVelocity	start velocity
toVelocity	end velocity

Returns: int total number of waypoints

◆ FindTimeToFenceViolation()

double TrajManager::FindTimeToFenceViolation	(	larcfm::Poly3D	polygon,
		larcfm::Vect3	so,
		larcfm::Velocity	vel
	)

private

Parameters

polygon	fence object
so	current position
vel	current velocity

Returns: double time to violation

◆ GetPlan()

larcfm::Plan * TrajManager::GetPlan ( std::string planID )

Parameters

planID

Returns: larcfm::Plan*

◆ GetPlanPosition()

larcfm::Position TrajManager::GetPlanPosition	(	std::string	planID,
		double	t
	)

Parameters

planID	id of plan
t	query time

Returns: larcfm::Position

◆ GetTotalWaypoints()

int TrajManager::GetTotalWaypoints ( std::string planID )

Parameters

planID

Returns: int

◆ GetWaypoint()

int TrajManager::GetWaypoint	(	std::string	planID,
		int	wpid,
		waypoint_t &	wp
	)

Parameters

planID	flight plan id
wpid	waypoint index
wp	[out] waypoint

Returns: int

◆ InputFlightPlan()

void TrajManager::InputFlightPlan	(	const std::string &	plan_id,
		const std::list< waypoint_t > &	waypoints,
		const double	initHeading,
		bool	repair,
		double	repairTurnRate
	)

Parameters

plan_id	id of plan
waypoints	list of waypoints in plan
initHeading	initial heading
repair	convert linear plan to kinematic plan
repairTurnRate	turn rate used for repairing

◆ InputGeofenceData() [1/2]

void TrajManager::InputGeofenceData	(	int	type,
		int	index,
		int	totalVertices,
		double	floor,
		double	ceiling,
		double	pos[][2]
	)

Parameters

type	fence type (keep in or keep out)
index	index of fence
totalVertices	total vertices in fences
floor	floor altitude
ceiling	ceiling altitude
pos	array of positions

◆ InputGeofenceData() [2/2]

void TrajManager::InputGeofenceData	(	int	type,
		int	index,
		int	totalVertices,
		double	floor,
		double	ceiling,
		std::vector< larcfm::Position > &	vertices
	)

Parameters

type	fence type (keep in or keep out)
index	index of fence
totalVertices	total vertices in fences
floor	floor altitude
ceiling	ceiling altitude
vertices	list of all vertices

◆ InputTraffic()

int TrajManager::InputTraffic	(	std::string	callsign,
		larcfm::Position &	position,
		larcfm::Velocity &	velocity,
		double	time
	)

Parameters

callsign
position
velocity
time

Returns: int

◆ MonitorTrajectory()

trajectoryMonitorData_t TrajManager::MonitorTrajectory	(	double	time,
		std::string	planID,
		larcfm::Position	pos,
		larcfm::Velocity	vel,
		int	nextWP1,
		int	nextWP2
	)

Parameters

time	current time
planID	id of plan
pos	current position
vel	current velocity
nextWP1	next wp on primary plan (Plan0)
nextWP2	next wp on second plan

Returns: trajectoryMonitorData_t

Consider the combined plan if planid not Plan0 and contain the "Plan" prefix

Initialize the output struct

Extract the plan that is being monitored

Create projections based on current position

Compute offsets with respect to given planID and primary plan

Correct for any time delay (-ve offsets[2] indicate a delay)

Keep in fence checks

Check fence conflict with current position
Check for projected fence conflict based on flightplan
Get position on plan for detected time to violation
If projected point on plan is outside fence, we have a real problem.

Keep out fence conflict

Check conflict based on current position
Check for projected fence conflict based on flightplan

Check for projected traffic conflict based on traffic flightplans

Check for projected traffic conflicts for state based traffic

Check for next feasible waypoint in the main plan

Check if next wp is before the conflict time
Skip BOT and MOT
Skip other tcps in a turn segment
The next segment should be sufficient long to facilitate plan capture
Check fence feasibility
Check line of sight to goal

◆ PlanToString()

std::string TrajManager::PlanToString ( std::string planID )

Parameters

planID

Returns: std::string

◆ ReadParamFromFile()

void TrajManager::ReadParamFromFile ( std::string config )

Parameters

config configuration file name

◆ SetPlanOffset()

void TrajManager::SetPlanOffset	(	std::string	planID,
		int	n,
		double	offset
	)

Parameters

planID	id of plan
n	index of waypoint from which timeshift should be applied
offset	time offset

◆ StringToPlan()

void TrajManager::StringToPlan	(	std::string	planID,
		std::string	inputString
	)

Parameters

planID
inputString

◆ UpdateDubinsPlannerParameters()

void TrajManager::UpdateDubinsPlannerParameters ( DubinsParams_t & params )

Parameters

params

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

Modules/Core/TrajectoryManager/TrajManager.hpp
Modules/Core/TrajectoryManager/TrajManager.cpp

Public Member Functions

Public Attributes

Private Member Functions

Private Attributes

Constructor & Destructor Documentation

◆ TrajManager()

Member Function Documentation

◆ CheckLineOfSightconflict()

◆ CombinePlan()

◆ ComputePlanOffsets()

◆ FindDubinsPath()

◆ FindPath()

◆ FindTimeToFenceViolation()

◆ GetPlan()

◆ GetPlanPosition()

◆ GetTotalWaypoints()

◆ GetWaypoint()

◆ InputFlightPlan()

◆ InputGeofenceData() [1/2]

◆ InputGeofenceData() [2/2]

◆ InputTraffic()

◆ MonitorTrajectory()

◆ PlanToString()

◆ ReadParamFromFile()

◆ SetPlanOffset()

◆ StringToPlan()

◆ UpdateDubinsPlannerParameters()