#include <ureactivepath.h>

Public Member Functions
void	clear ()

void	copy (UReactivePath *source)

bool	findObstAvoidRoute (UReacObstGrps obsts, UReacRoadLines roads, UAvoidPathPool *paths, bool ignoreObstacles, bool directTestOnly, bool driveon, double turnRad)

UManSeq *	getManSeq ()

double	getManTime ()

UPose	getMidPose (int i)

int	getMidPosesCnt ()

bool	getPathUsed ()

double	getRouteAngleExit ()

double	getRouteAngleFirst ()

int	getRouteCnt ()

double	getRouteDirectToExitDist ()

double	getRouteDist ()

double	getRouteSD ()

bool	isACrash ()

bool	isRouteFound ()

bool	isValid ()

void	print (const char prestring, char buff, const int buffCnt)

void	setExitPose (UPoseV pose)

void	setPathUsed (bool value)

void	setRouteInvalid ()

void	setRouteStats (UPose odoPose)

void	setValid (bool value)

	UReactivePath ()

	~UReactivePath ()

Protected Member Functions
bool	avoidObst (UReacObstGrps obsts, UAvoidPathPool pathPool, UObstInfo obstSideList, bool obstSideSpawn, double maxVel, double maxAcc, double maxTurnAcc, double minTurnRad, int *spawnCnt, int spawnNestLevel, bool directTestOnly, bool useDriveon, double turnRad)

bool	getMidPoseHere (UObstacle obst1, UObstacle obst2, UPosition minPos1, UPosition minPos2, int side, UPose midPose, double width, UPose fromPose)

UObstacle *	getNearObstacle (UReacObstGrps obsts, int minVertexCnt, double dist, UPosition pos, int vertex, UObstInfo exclude, UObstacle notThis)

UPoseV	getTangentMidPose (int minIdx1, int minIdx2, int side, UPosition minDistPos, bool newEndPoint, double atDistance)

bool	getWorstVertex (UObstacle obs, int minVertex, int end, int side, double minDist, UPosition minDistPos, UManPPSeq mpp)

bool	getWorstVertex2 (UObstacle obs, int side, double minDist, UPosition minDistPos, UManPPSeq mpp)

int	testForObstacleNearExit (UReacObstGrps obsts, const int usedEdge, UPosition posExit, ULineSegment *seg, double obstDist, int n)

Private Attributes
bool	crash

double	distExitDirect

bool	endsInObst

UPosition	endsInObstHere

UPoseV	exitPose

int	intervalsCnt

UManSeq *	man

UPose	midPoses [MAX_MID_POSES]

int	midPosesCnt

bool	pathUsed

double	routeAngleExit

double	routeAngleFirst

double	routeDist

double	routeSD

bool	valid

Static Private Attributes
static const int	MAX_MID_POSES = 50

Detailed Description

A Laser path is a series of overlapping traversable segments also called a corrodor. The corridor is an array of pointers to UPassIntervals. The first interval is the far from the robot.

Constructor & Destructor Documentation

UReactivePath::UReactivePath ( )

Constructor

References UReacRoadLines::clear().

UReactivePath::~UReactivePath ( )

Destructor

Member Function Documentation

bool UReactivePath::avoidObst	(	UReacObstGrps *	obsts,
		UAvoidPathPool *	pathPool,
		UObstInfo	obstSideList,
		bool	obstSideSpawn,
		double	maxVel,
		double	maxAcc,
		double	maxTurnAcc,
		double	minTurnRad,
		int *	spawnCnt,
		int	spawnNestLevel,
		bool	directTestOnly,
		bool	useDriveon,
		double	turnRad
	)

protected

Expand this manoeuvre as needed to avoid the obstacles in the 'obsts' list, using the first 'obstsCnt' of the obstackle groups only. The obstacles are to be passed in the easiest way, unless the obstycle is noted in the 'obstSideList', where a specific side may be listed.

Parameters

useDriveon	replaces the pose destination with a line destination - as the drive and driveon commands.
turnRad	is the turn radius to use. true if at least one path is generated. More paths may be spawned if 'obstSideSpawn' is true (and there is space in 'pathPool' for more paths.

Todo:: create alternative path

References a, avoidObst(), copy(), UPosition::dist(), UPolygon::getCogXY(), UPolygon::getCogXYmaxDist(), UAvoidPathPool::getCrashTest(), UPose::getDistance(), UPolygon::getDistanceXYsigned2(), UReacObstGrps::getGroup(), UReacObstGrps::getGroupsCnt(), UAvoidPathPool::getMaxAvoidLoops(), UAvoidPathPool::getMaxNestedLevels(), UAvoidPathPool::getMaxSpawnCnt(), UAvoidPathPool::getNewPath(), UObstacleGroup::getObstacle(), UObstInfo::getObstInfo(), UAvoidPathPool::getObstMinDist(), UAvoidPathPool::getObstMinMinDist(), UObstacleGroup::getObstsCnt(), UPolygon::getPointsCnt(), UPose::getPos(), UPolygon::getSegment(), UPose::h, UPolygon::isInsideConvex(), UObstacle::isValid(), limitToPi(), maxd(), maxi(), mind(), mini(), ROBOTLENGTH, ROBOTWIDTH, UPosition::set(), UObstInfo::setObstInfo(), UPoseV::setVel(), UPose::x, and UPose::y.

Referenced by avoidObst().

void UReactivePath::clear ( void )

Clear path

Referenced by UAvoidPathPool::findPathToHere().

void UReactivePath::copy ( UReactivePath * source )

Make a copy of the source path into this path structure. Route statistics are not copied. and used flag is set to false. Road values and passable intervals are copied fully. Manoeuvre sequence is copied fully.

References UReacRoadLines::clear(), getManSeq(), getMidPose(), getMidPosesCnt(), getRouteAngleExit(), getRouteAngleFirst(), getRouteDist(), getRouteSD(), isACrash(), and isValid().

Referenced by avoidObst().

bool UReactivePath::findObstAvoidRoute	(	UReacObstGrps *	obsts,
		UReacRoadLines *	roads,
		UAvoidPathPool *	paths,
		bool	ignoreObstacles,
		bool	directTestOnly,
		bool	driveon,
		double	turnRad
	)

Find route - using circular distance from objects. 'edge' can have one of the following values: WPF_ROUTE_GO_RIGHT, WPF_ROUTE_GO_LEFT WPF_ROUTE_GO_TOP or WPF_ROUTE_GO_POSITION. 'Destination' position is only relevant if edge == WPF_ROUTE_GO_POSITION. Dist from edge (distFromEdge) is desired distance for edge at exit of path (relevant for left and right edge only) Returns true if a route is found. The route is returned in the route array (in odometry map coordinates). Find route - using circular distance from objects. 'edge' can have one of the following values: WPF_ROUTE_GO_RIGHT, WPF_ROUTE_GO_LEFT WPF_ROUTE_GO_TOP or WPF_ROUTE_GO_POSITION. 'Destination' position is only relevant if edge == WPF_ROUTE_GO_POSITION. Dist from edge (distFromEdge) is desired distance for edge at exit of path (relevant for left and right edge only) Returns true if a route is found.

Parameters

directTestOnly	test direct path to destination only return true is possible
useDriveon	replaces the pose destination with a line destination - as the drive and driveon commands.
turnRad	is the turn radius to use when using drive-on command.
gD	is the distance (from line) gain. approximated turn radius is PI/2 * gA / gD. The route is returned in the route array (in odometry map coordinates).

References dist, UAvoidPathPool::getCrashTest(), UPose::getDistance(), UAvoidPathPool::getMaxAcc(), UAvoidPathPool::getMaxTurnAcc(), UAvoidPathPool::getMinTurnRad(), UAvoidPathPool::getStartPoseVel(), UPoseV::getVel(), and maxd().

Referenced by UAvoidPathPool::findPathToHere().

UManSeq* UReactivePath::getManSeq ( )

inline

Get manoeuver sequence for planned drive path

Referenced by copy(), UResAvoid::findPathToHere(), UAvoidPathPool::findPathToHere(), UAvoidPathPool::getNewPath(), and UFunctionAvoid::sendCurrentPath().

double UReactivePath::getManTime ( )

inline

Get manoeuver sequence drive time

Referenced by UAvoidPathPool::findPathToHere().

UPose UReactivePath::getMidPose ( int i )

inline

Get a pointer to one of the crash test mid-poses

References dist.

Referenced by copy(), and UFunctionAvoid::sendCurrentPath().

bool UReactivePath::getMidPoseHere	(	UObstacle *	obst1,
		UObstacle *	obst2,
		UPosition	minPos1,
		UPosition	minPos2,
		int	side,
		UPose *	midPose,
		double *	width,
		UPose	fromPose
	)

protected

Get position and heading for the best passage points between these two obstacles. Iterate the result from the midpoint between 'minPos1' and 'minPos2'. Returns result in 'midPose'. 'fromPose' must be set to a point before the new midPoint, to get the heading in the right main direction. The position is based on distance to the two involved obstacles, and heading is set in order to pass at a right angle to the line between the obstacles. Passage width is returned in 'width' (if not NULL). Returns true if possible, i.e. at least 'ROBOTWIDTH' of safe passage.

References a, UPolygon::getDistanceXYsigned2(), UPolygon::getSegment(), ULine::getXYHeading(), UPose::h, limitToPi(), ROBOTWIDTH, UPosition::x, UPose::x, UPosition::y, and UPose::y.

int UReactivePath::getMidPosesCnt ( )

inline

Get count of crash test mid-poses

Referenced by copy(), and UFunctionAvoid::sendCurrentPath().

UObstacle * UReactivePath::getNearObstacle	(	UReacObstGrps *	obsts,
		int	minVertexCnt,
		double	dist,
		UPosition	pos,
		int *	vertex,
		UObstInfo *	exclude,
		UObstacle *	notThis
	)

protected

Get closest vertex to the position in 'minDistPos', then distance must be less than 'dist'. The closest vertex has number 'vertex' on exit. Returns NULL, if no obstacle if found near enough. Do not test obstacles in the 'exclude' list and not 'notThis' either.

References UReacObstGrps::getGroup(), UReacObstGrps::getGroupsCnt(), UObstacleGroup::getObstacle(), UObstInfo::getObstInfo(), UObstacleGroup::getObstsCnt(), UPolygon::getPoint(), UPolygon::getPointsCnt(), sqr(), UPosition::x, and UPosition::y.

bool UReactivePath::getPathUsed ( )

inline

Is this path actually used for robot control

Referenced by UFunctionAvoid::sendCurrentPath().

double UReactivePath::getRouteAngleExit ( )

inline

Get route angle to scan exit point relative to present robot heading (in radians positive is CCV, zero is forward)

Referenced by copy().

double UReactivePath::getRouteAngleFirst ( )

inline

Get route angle to first route point relative to present robot heading (in radians positive is CCV, zero is forward)

Referenced by copy().

int UReactivePath::getRouteCnt ( )

inline

Get number op points (poses) in route

double UReactivePath::getRouteDirectToExitDist ( )

inline

Get direct distance to exit posture. Is calculated by setRouteStats().

double UReactivePath::getRouteDist ( )

inline

Get route distance or length from robot to exit point

Referenced by copy().

double UReactivePath::getRouteSD ( )

inline

Get route SD relative to direct path

Referenced by copy().

UPoseV UReactivePath::getTangentMidPose	(	int	minIdx1,
		int	minIdx2,
		int	side,
		UPosition	minDistPos,
		bool	newEndPoint,
		double	atDistance
	)

protected

Find in this path the best linefit of the road for this 'side' of the road. Side may be 0=left, 1=Right, 2=Center of road. Returns true if line segment is found, with optional variance returned at var pointer (if var != NULL). Get a mid waypoint from this obstacle position. If tangent lines cross (before touching the circle), then a tangent point in the direction to point on the path closest to the obstacle, the 'offendedPoint', is used.

References UPosition::dist(), UPosition::getTangentPointXY(), UPose::h, UPosition::x, UPose::x, UPosition::y, and UPose::y.

bool UReactivePath::getWorstVertex	(	UObstacle *	obs,
		int	minVertex,
		int	end,
		int	side,
		double *	minDist,
		UPosition *	minDistPos,
		UManPPSeq *	mpp
	)

protected

Find the most distant vertex in this obstacle, starting at 'minVertex' the 'minVertex' segment is hit at the 'end' = 0=mid, 1=start i.e. the vertex, 2= other end i.e. vertex+1. Return the distance (most likely negatve) in '*dist' and the new position in '*minDistPos'. The manoeuvre used for distance-testing is in 'mpp'.

References UManPPSeq::getDistanceXYSigned(), UPolygon::getPoint(), UPolygon::getPointsCnt(), and mind().

bool UReactivePath::getWorstVertex2	(	UObstacle *	obs,
		int	side,
		double *	minDist,
		UPosition *	minDistPos,
		UManPPSeq *	mpp
	)

protected

Find the most distant vertex in this obstacle, signed relative tom manoeuvre part in 'mpp'. Side is the side that is counted positive 0=left, 1=right. Return the distance (most likely negatve) in '*dist' and the new position in '*minDistPos'. Returns true.

References UManPPSeq::getDistanceXYSigned(), UPolygon::getPoint(), and UPolygon::getPointsCnt().

bool UReactivePath::isACrash ( )

inline

Is the route maintained for crash analysis

Referenced by copy(), UAvoidPathPool::findPathToHere(), and UFunctionAvoid::sendCurrentPath().

bool UReactivePath::isRouteFound ( )

inline

Test if the route ends in a wall. Test movement of passable interval relative to last scan. if less movement than a movementLimit, then wall counter is increased. Returns wall count. Is a local route avoiding obstacle available. Returns true if a route point is available

bool UReactivePath::isValid ( )

inline

Is route valid

Referenced by copy(), UAvoidPathPool::findPathToHere(), UAvoidPathPool::getNewPath(), UAvoidPathPool::getValidPathsCnt(), and UFunctionAvoid::sendCurrentPath().

void UReactivePath::print	(	const char *	prestring,
		char *	buff,
		const int	buffCnt
	)

Print some status for the path to this buffer

void UReactivePath::setExitPose ( UPoseV pose )

inline

set exit pose

Referenced by UAvoidPathPool::findPathToHere().

void UReactivePath::setPathUsed ( bool value )

inline

Is this path actually used for robot control

Referenced by UAvoidPathPool::findPathToHere(), and UAvoidPathPool::getNewPath().

void UReactivePath::setRouteInvalid ( )

inline

Set path invalid and remove all existing manoeuvres;

void UReactivePath::setRouteStats ( UPose odoPose )

Calculate performance statistics for a route line. deviation (SD) from a direct line from robot to route endpoint, and angle offset relative to current robot position.

References UPose::h, limitToPi(), UPosition::set(), UPosition::x, UPose::x, UPosition::y, and UPose::y.

void UReactivePath::setValid ( bool value )

inline

Set route valid flag

Referenced by UAvoidPathPool::getNewPath().

int UReactivePath::testForObstacleNearExit	(	UReacObstGrps *	obsts,
		const int	usedEdge,
		UPosition *	posExit,
		ULineSegment *	seg,
		double	obstDist,
		int	n
	)

protected

Test for near obstacle and move exit point away from obstacle (closer to other end), if possible. If not possible or no obstacle, then 'posExit' is unchanged. 'n' is an iteration count, as the function may iterate a solution (max = 3). Returns 0 if position is OK as is. Returns 1 is position is changed OK. Returns 2 if no valid position is found.

References UPolygon::getCogXY(), UPolygon::getCogXYmaxDist(), ULineSegment::getDistanceXYSigned(), UPolygon::getDistanceXYsigned2(), UReacObstGrps::getGroup(), UReacObstGrps::getGroupsCnt(), UObstacleGroup::getObstacle(), UObstacleGroup::getObstsCnt(), ULine::getPositionOnLine(), sqr(), WPF_ROUTE_GO_LEFT, WPF_ROUTE_GO_RIGHT, UPosition::x, and UPosition::y.

Member Data Documentation

bool UReactivePath::crash

private

Is route not valid, but maintained for crash analysis

double UReactivePath::distExitDirect

private

Direct distance from robot to exit posture. Is used to evaluate best route distance

bool UReactivePath::endsInObst

private

Flag for desired exit point is in an obstacle. This may be the case in especially DIRECT type movement, and may require special treatmnet (tell calculator).

UPosition UReactivePath::endsInObstHere

private

Position where route enters obstacle

UPoseV UReactivePath::exitPose

private

Desired exit pose and speed

int UReactivePath::intervalsCnt

private

Series of passable intervals Number of scans in path

UManSeq* UReactivePath::man

private

Route points to travel along this path. The first point in route is Count of points in route New manoeuvre sequence

const int UReactivePath::MAX_MID_POSES = 50

staticprivate

Size of crash test array with tested mid-points.

UPose UReactivePath::midPoses[MAX_MID_POSES]

private

Points used as mid-poses - deleted or not. Used for crash analysis only.

int UReactivePath::midPosesCnt

private

Count of used mid-poses

bool UReactivePath::pathUsed

private

Were this ths path that actualy were used by the way-point-controller

double UReactivePath::routeAngleExit

private

Route orientation relative to the robot. I.e. angle to the line from the robot to the route endpoint (approx 2.5 meter ahead).

double UReactivePath::routeAngleFirst

private

Route orientation relative to the robot. I.e. angle to the line from the robot to the first waypoint on route.

double UReactivePath::routeDist

private

Route total distance from robot to exit-point

double UReactivePath::routeSD

private

Line deviation relative to a line from the robot to the route endpoint

bool UReactivePath::valid

private

Is route valid - if not, should be deleted

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

Public Member Functions

Protected Member Functions

Private Attributes

Static Private Attributes

Detailed Description

Constructor & Destructor Documentation

Member Function Documentation

Member Data Documentation