#include <umanppseq.h>

Public Member Functions
bool	add (UManoeuvre *man)

UPoseV	calcEndPose (UPoseV startPose)

double	calcEndVel (double andManTime, double manDist)

UManoeuvre *	extract (int point)

void	fprint (FILE fd, const char prestring)

double	getDistanceXYSigned (UPosition pos, int where, bool posIsRight, UPose pHit, double atT, int atMan=NULL)

double	getEndDistTo (UPosition pos)

UPosition	getEndPos ()

UPoseV	getEndPoseV ()

double	getEndVel ()

UManoeuvre *	getMan (int point)

double	getManDist ()

double	getManTime ()

int	getMaxManCnt ()

double	getMaxTurnArc ()

double	getMinDistanceXYSigned (ULineSegment seg, int whereOnSeg, UPosition posOnSeg, bool posIsRight, int whereOnMan, UPose *poseOnMan)

UPoseV	getPoseV (double atManTime, UPoseV startPose)

UPoseV	getPoseV (int mn)

UPoseV	getPoseV (double atManTime)

int	getSeqCnt ()

double	getStartDistTo (UPosition pos)

UPosition	getStartPos ()

UPoseV	getStartPoseV ()

bool	insert (UManoeuvre *man, int point)

bool	isFirstFixed ()

bool	isLastFixed ()

bool	isValid ()

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

void	setEndPoseV (UPoseV poseV)

void	setEndVel (double vel)

void	setFirstFixed (bool value)

void	setLastFixed (bool value)

void	setManDist (double value)

void	setManTime (double value)

bool	setMaxVel (double maxVel)

void	setStartPoseV (UPoseV poseV)

void	truncate (int i)

void	truncate (UPosition pos, double preDist)

	UManPPSeq ()

	~UManPPSeq ()

Protected Attributes
UPoseV	endPose

double	endVel

bool	fixedFirstPose

bool	fixedLastPose

double	manDistance

double	manTime

UManoeuvre *	seq [MAX_MAN_CNT]

int	seqCnt

UPoseV	startPose

Static Protected Attributes
static const int	MAX_MAN_CNT = 20

Detailed Description

Holds the manoeuvres needed to get from one pose to another. The start and end pose and velocity is stored too, to enable revalculation of manoeuvre if start-speed is changed.

Author: Christian Andersen

Constructor & Destructor Documentation

UManPPSeq::UManPPSeq ( )

Constructor

References endVel, fixedFirstPose, fixedLastPose, manDistance, manTime, MAX_MAN_CNT, seq, and seqCnt.

UManPPSeq::~UManPPSeq ( )

Destructor

References seq, and seqCnt.

Member Function Documentation

bool UManPPSeq::add ( UManoeuvre * man )

Add a manoeuvre at end of sequence. Returns true if added.

References getMaxManCnt(), seq, and seqCnt.

Referenced by UManSeq::copy(), UManSeq::expandAddManALA(), UManSeq::expandAddManLALA(), UManSeq::expandManDriveon(), UAvoidPath2::extendManoeuvre(), and USmlTag::getManSeq().

UPoseV UManPPSeq::calcEndPose ( UPoseV startPose )

Calculate end pose using by predicting from manoeuvre sequence.

References UManoeuvre::getEndPoseV(), seq, seqCnt, and startPose.

Referenced by UManSeq::expandManDriveon(), getPoseV(), UManSeq::replaceManDriveon(), and truncate().

double UManPPSeq::calcEndVel	(	double *	andManTime,
		double *	manDist
	)

Calculate end velocity, and manoeuvre time as well if andManTime != NULL.

References UManoeuvre::getManTime(), UPoseV::getVel(), seq, seqCnt, and startPose.

Referenced by UManSeq::expandMan(), UManSeq::expandManDriveon(), and getPoseV().

UManoeuvre * UManPPSeq::extract ( int point )

Extract a manoeuvre from sequence, moving the rest down as needed. Returns NULL if index is not inside valid range. Returns a pointer to the removed manoeuvre.

References seq, and seqCnt.

Referenced by UManSeq::releaseAll(), and UManSeq::truncate().

void UManPPSeq::fprint	(	FILE *	fd,
		const char *	prestring
	)

Print status for manoeuvre section

References endPose, endVel, UPoseV::fprint(), UPoseV::getVel(), manDistance, manTime, seq, seqCnt, and startPose.

Referenced by UManSeq::expandManDriveon(), UManSeq::fprint(), and getPoseV().

double UManPPSeq::getDistanceXYSigned	(	UPosition	pos,
		int *	where,
		bool	posIsRight,
		UPose *	pHit,
		double *	atT,
		int *	atMan = `NULL`
	)

Get distance to this position from the manoeuvre lines

Parameters

pos	is the posittion to be tested for distance.
where	is the closest position: 0=point on line, 1= first point, 2= other end (end point).
posIsRight	if true, then positive is to the right of segment - seen from first end in direction towards other end.
pHit	is the pose on the manoeuvre sequence closest to the position If where is 0, then 'pHit' is set to the closest position to the path. If where != 0, then pHit will be unchanged.
atT	is set to the distance into full manoeuvre sequence - in m along the curve.
atMan	is the index to the hit manoeuvre (set if not NULL only)

Returns: distance to either end or to a point on line whatever is closest.

References UManoeuvre::getDistance(), UManoeuvre::getDistanceXYSigned(), UManoeuvre::getEndPose(), UPose::getMapToPose(), UPose::getPoseToMapPose(), seq, seqCnt, and startPose.

Referenced by UAvoidPath2::getClosestObst2(), UManSeq::getDistanceXYSigned(), UReactivePath::getWorstVertex(), UReactivePath::getWorstVertex2(), setManTime(), and truncate().

double UManPPSeq::getEndDistTo ( UPosition pos )

inline

Get distance to end pose from this position

References endPose, UPosition::x, UPose::x, UPosition::y, and UPose::y.

UPosition UManPPSeq::getEndPos ( )

inline

Get end position (z is set to 0.0)

References endPose, and UPose::getPos().

Referenced by UManSeq::getDeviationFromDirect().

UPoseV UManPPSeq::getEndPoseV ( )

inline

Get end pose (poseV)

References endPose.

Referenced by USmlTag::codeManPPSeq(), UManSeq::copy(), UManSeq::expandAddManALA(), UManSeq::expandAddManLALA(), UAvoidPath2::extendManoeuvre(), UAvoidPath2::getClosestObst2(), UManSeq::getDistanceXYSigned(), UManSeq::getEndPoseV(), UManSeq::getMinDistanceXYSigned(), UClientFuncLaserGui::paintPathData(), UManSeq::replaceMan(), UManSeq::replaceManDriveon(), UResSmrCtl::run(), and UResSmrCtl::sendNewManoeuvreToSMR().

double UManPPSeq::getEndVel ( )

inline

Get acheived end velocity. Positive is forward. [m/s].

References endVel.

Referenced by USmlTag::codeManPPSeq(), UManSeq::copy(), UManSeq::replaceMan(), and UManSeq::replaceManDriveon().

UManoeuvre * UManPPSeq::getMan ( int point )

Get a pointer to a manoeuvre at this point

References seq, and seqCnt.

Referenced by USmlTag::codeManPPSeq(), UManSeq::copy(), UAvoidPath2::getClosestObst2(), UAvoidPath2::getManAsPolygon(), UPaintManoeuvre::makeManCloud(), UNavPaint::paintManData(), UClientFuncLaserGui::paintPathData(), UManSeq::removeLastArc(), and UResSmrCtl::sendNewManoeuvreToSMR().

double UManPPSeq::getManDist ( )

inline

Set total manoeuvre distance

References manDistance.

Referenced by UManSeq::copy(), UManSeq::getDistance(), UManSeq::getDistanceXYSigned(), UAvoidPath2::getManAsPolygon(), and UManSeq::getMinDistanceXYSigned().

double UManPPSeq::getManTime ( )

inline

Get time spend in manoeuvre.

References manTime.

Referenced by USmlTag::codeManPPSeq(), UManSeq::copy(), UManSeq::getPoseV(), and UManSeq::getTime().

int UManPPSeq::getMaxManCnt ( )

inline

Get maximum number of manoeuvre pointers allowed

References MAX_MAN_CNT.

Referenced by add(), UManSeq::expandAddManALA(), UManSeq::expandAddManLALA(), and insert().

double UManPPSeq::getMaxTurnArc ( )

Get the maximum turn angle in this sequence of manoeuvres. Returns 0.0 if sequence has no arcs.

References UManoeuvre::getManType(), UManArc::getTurnAngle(), UManoeuvre::MAN_ARC, seq, and seqCnt.

Referenced by UManSeq::getMaxTurnArc(), and getStartDistTo().

double UManPPSeq::getMinDistanceXYSigned	(	ULineSegment *	seg,
		int *	whereOnSeg,
		UPosition *	posOnSeg,
		bool	posIsRight,
		int *	whereOnMan,
		UPose *	poseOnMan
	)

Get shortest distance to this manoeuvre route to 'seg'. If 'posIsRight', then the distance is positive to the right of the route. The closest point on the segnment is returned in 'posOnSeg'. If the closest point on the segment streach, then whereOnSeg = 0, if the ref end of segment, then whereOnSeg = 1, other end is 2. the closest point in the route is returned in 'poseOnSeg'. If the closest point on the manoeuvre is between endpoints, then whereOnMan = 0, if the first end of manoeuvre, then whereOnMan = 1, last end is 2.

References UManoeuvre::getEndPose(), UPose::getMapToPose(), UManoeuvre::getMinDistanceXYSigned(), ULineSegment::getOtherEnd(), UPose::getPoseToMap(), UPose::getPoseToMapPose(), ULine::pos, seq, seqCnt, ULineSegment::setFromPoints(), startPose, ULine::vec, and UPosition::z.

Referenced by UManSeq::getMinDistanceXYSigned(), and setManTime().

UPoseV UManPPSeq::getPoseV	(	double	atManTime,
		UPoseV	startPose
	)

Get poseV at time, using specified start pose

References endPose, UManoeuvre::getManTime(), UPoseV::getVel(), seq, seqCnt, and startPose.

Referenced by getPoseV(), and setEndVel().

UPoseV UManPPSeq::getPoseV ( int mn )

Get pose at end of this sequence.

Parameters

mn	is the index of the manoeuvre of which the end pose is returned.

Returns: end pose of the indicated manoeuvre.

References seq, seqCnt, and startPose.

UPoseV UManPPSeq::getPoseV ( double atManTime )

inline

Get poseV at time, using storet start pose

References calcEndPose(), calcEndVel(), fprint(), getPoseV(), and print().

int UManPPSeq::getSeqCnt ( )

inline

Get number of manoeuvres in sequence

References seqCnt.

Referenced by USmlTag::codeManPPSeq(), UManSeq::copy(), UManSeq::expandAddManALA(), UManSeq::expandAddManLALA(), UAvoidPath2::getClosestObst2(), UAvoidPath2::getManAsPolygon(), UPaintManoeuvre::makeManCloud(), UNavPaint::paintManData(), UClientFuncLaserGui::paintPathData(), UManSeq::releaseAll(), UManSeq::removeLastArc(), UResSmrCtl::sendNewManoeuvreToSMR(), and UManSeq::truncate().

double UManPPSeq::getStartDistTo ( UPosition pos )

inline

Get distance to end pose from this position

References getMaxTurnArc(), UPosition::x, UPose::x, UPosition::y, and UPose::y.

UPosition UManPPSeq::getStartPos ( )

inline

Get start position (z is set to 0.0)

References UPose::getPos(), and startPose.

UPoseV UManPPSeq::getStartPoseV ( )

inline

Get start pose (poseV)

References startPose.

Referenced by USmlTag::codeManPPSeq(), UManSeq::copy(), UManSeq::expandAddManALA(), UManSeq::expandAddManLALA(), UManSeq::expandManDriveon(), UAvoidPath2::getClosestObst2(), UManSeq::getDistanceFromEndPoseLine(), UManSeq::getDistanceXYSigned(), UAvoidPath2::getManAsPolygon(), UManSeq::getMinDistanceXYSigned(), UManSeq::getStartPoseV(), UNavPaint::paintManData(), UClientFuncLaserGui::paintPathData(), UManSeq::replaceMan(), UManSeq::replaceManDriveon(), and UResSmrCtl::sendNewManoeuvreToSMR().

bool UManPPSeq::insert	(	UManoeuvre *	man,
		int	point
	)

Insert manoeuvre at this point in sequence Returns true if inserted. if point is larger than current size, it is added at end. if (point is 0 or less the manoeuvre is added at beginning. Returns true if inserted (enough space).

References getMaxManCnt(), maxi(), mini(), seq, and seqCnt.

bool UManPPSeq::isFirstFixed ( )

inline

Is first pose fixed - e.g. passing a narrow passage

References fixedFirstPose.

bool UManPPSeq::isLastFixed ( )

inline

Is last pose fixed - e.g. passing a narrow passage

References fixedLastPose.

bool UManPPSeq::isValid ( )

inline

Is this pose-to-pose sequence valid, i.e. are there ant defined manoeuvres.

References seqCnt.

Referenced by UManSeq::isValid().

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

Print status to buffer string

References endPose, endVel, UPoseV::getVel(), manDistance, manTime, UManoeuvre::print(), UPoseV::print(), seq, seqCnt, and startPose.

Referenced by getPoseV(), and UManSeq::print().

void UManPPSeq::setEndPoseV ( UPoseV poseV )

inline

Set end pose (poseV)

References endPose.

Referenced by UManSeq::copy(), UManSeq::expandAddManALA(), UManSeq::expandMan(), UManSeq::expandManDriveon(), UAvoidPath2::extendManoeuvre(), USmlTag::getManSeq(), and UManSeq::replaceManDriveon().

void UManPPSeq::setEndVel ( double vel )

inline

Set acheived end velocity. Positive is forward. [m/s].

References endVel, getPoseV(), and startPose.

Referenced by UManSeq::copy(), UManSeq::expandMan(), UManSeq::expandManDriveon(), and USmlTag::getManSeq().

void UManPPSeq::setFirstFixed ( bool value )

inline

Set first pose as fixed, i.e. should not be changed by manoeuvre planning, even if a new waypoint is close by.

References fixedFirstPose.

Referenced by UManSeq::releaseAll().

void UManPPSeq::setLastFixed ( bool value )

inline

Set last pose as fixed, i.e. should not be changed by manoeuvre planning, even if a new waypoint is close by.

References fixedLastPose, setMaxVel(), and truncate().

Referenced by UManSeq::releaseAll(), UManSeq::replaceMan(), and UManSeq::replaceManDriveon().

void UManPPSeq::setManDist ( double value )

inline

Set total manoeuvre distance

References manDistance.

Referenced by UManSeq::copy(), UManSeq::expandMan(), and UManSeq::expandManDriveon().

void UManPPSeq::setManTime ( double value )

inline

Set time to complete manoeuvre.

References getDistanceXYSigned(), getMinDistanceXYSigned(), and manTime.

Referenced by UManSeq::copy(), UManSeq::expandMan(), UManSeq::expandManDriveon(), and USmlTag::getManSeq().

bool UManPPSeq::setMaxVel ( double maxVel )

workaround to limit max velocity returns true if all velocities are less than 'maxVel'

References endVel, UPoseV::getVel(), seq, seqCnt, UPoseV::setVel(), and startPose.

Referenced by UResSmrCtl::sendNewManoeuvreToSMR(), and setLastFixed().

void UManPPSeq::setStartPoseV ( UPoseV poseV )

inline

Set start pose (poseV)

References startPose.

Referenced by UManSeq::copy(), UManSeq::expandAddManALA(), UManSeq::expandMan(), UManSeq::expandManDriveon(), and USmlTag::getManSeq().

void UManPPSeq::truncate ( int i )

Truncate this manoeuvre and the rest - the structures themself must be removed by the caller.

Parameters

i	index to the first structure to remove

References seq, and seqCnt.

Referenced by UAvoidPath2::convertToManSeq(), UManSeq::removeLastArc(), setLastFixed(), and truncate().

void UManPPSeq::truncate	(	UPosition	pos,
		double	preDist
	)

Truncate manoeucre at a distance before a given position. The closest position on the path is found, and a preDistance before that the path is truncated. It is used to stop a planned route, if an obstacle can not be avoided by other means.

Parameters