#include <umanseq.h>

Inheritance diagram for UManSeq:

Public Member Functions
bool	addMan (UPoseV fromPose, UPoseV toPose, double maxAcc, double maxTurnAcc, double minTurnRad, double *okVel=NULL)

bool	addManDriveon (UPoseV fromPose, UPoseV toPose, double turnRad)

UManPPSeq *	addP2P ()

bool	copy (UManSeq *source)

bool	expandMan (UPoseV fromPose, UPoseV toPose, bool last, double maxAcc, double maxTurnAcc, double minTurnRad, UManPPSeq ppSeq, double okVel)

bool	expandManDriveon (UPoseV fromPose, UPoseV toPose, double turnRad, UManPPSeq *ppSeq)

void	fprint (FILE fd, const char prestr)

void	freeAll ()

virtual const char *	getDataType ()

double	getDeviationFromDirect ()

double	getDistance ()

double	getDistanceFromEndPoseLine (double *maxHeading)
	Calculate the distance from the end-pose line to the point on the manoeuvre furthest away from the line. Only the start/end points of each manoeuvere is used, so if one of the pose to pose manoeuvres has an almost full circle, then the circle is not counted. More...

double	getDistanceXYSigned (UPosition pos, int where, bool posIsRight, double maxDist, UPose pHit, int idx, double t)

UPoseV	getEndPoseV ()

double	getMaxTurnArc ()

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

double	getNeededAcc (double startVel, double endVel, double distance)

double	getNeededDist (double startVel, double endVel, double acc)

UManoeuvre *	getNewArc (double radius, double angle, bool left, double acceleration, double targetVel)

UManoeuvre *	getNewArc (double radius, double angle, double acceleration, double targetVel)

UManoeuvre *	getNewLine (double dist, double acceleration, double targetVel)

UManoeuvre *	getNewStop ()

UManPPSeq *	getP2P (int idx)

int	getP2PCnt ()

UPoseV	getPoseV (double atManTime)

UPoseV	getStartPoseV ()

double	getTime ()

bool	isValid ()

const char *	print (const char prestr, char buff, const int buffCnt)

void	releaseAll (UManPPSeq *ppseq)

void	releaseAllMan ()

void	releaseLast ()

void	removeLastArc ()

bool	replaceMan (UPoseV midPose, bool fixate, double maxAcc, double maxTurnAcc, double minTurnRad, int fromIdx, int toIdx, bool newEndpoint, double *okVel)

bool	replaceManDriveon (UPoseV midPose, bool fixate, double turnRad, int fromIdx, int toIdx, bool newEndpoint)

int	reusableCnt ()

void	truncate (UManPPSeq *ppseq, int newCnt)

	UManSeq ()

	~UManSeq ()

Public Member Functions inherited from ULock
bool	lock ()

void	lockInit ()

void	post ()

bool	tryLock ()

bool	tryWait ()

	ULock ()

void	unlock ()

bool	wait ()

	~ULock ()

Public Member Functions inherited from UDataBase
bool	isA (const char *typeString)

virtual bool	isAlsoA (const char *typeString)

virtual void	snprint (const char preString, char buff, const int buffCnt)

	UDataBase ()

virtual	~UDataBase ()

Protected Member Functions
bool	expandAddManALA (UManPPSeq ppSeq, bool last, double maxAcc, double maxTurnAcc, double okVel)

bool	expandAddManLALA (UManPPSeq *ppSeq, bool last, double maxAcc, double maxTurnAcc, double minTurnRad)

void	release (UManoeuvre *toRelease)

UManoeuvre *	requestObj (int manType)

Protected Attributes
UManPPSeq *	p2p [MAX_PP_MAN_CNT]

int	p2pCnt

Static Protected Attributes
static const int	MAX_PP_MAN_CNT = 100

Private Attributes
UManoeuvre *	reuse [MAX_REUSABLE_LINE_OBJ]

int	reuseCnt

Static Private Attributes
static const int	MAX_REUSABLE_LINE_OBJ = 100

Detailed Description

A sequence of manoeuvres and related functions

Author: Christian Andersen

Constructor & Destructor Documentation

UManSeq::UManSeq ( )

Constructor

References MAX_PP_MAN_CNT, MAX_REUSABLE_LINE_OBJ, p2p, p2pCnt, reuse, and reuseCnt.

UManSeq::~UManSeq ( )

Destructor

References freeAll(), and releaseAllMan().

Member Function Documentation

bool UManSeq::addMan	(	UPoseV	fromPose,
		UPoseV	toPose,
		double	maxAcc,
		double	maxTurnAcc,
		double	minTurnRad,
		double *	okVel = `NULL`
	)

Adds a manoeuvre at the end of the current sequence. The from pose should be the same as the toPose of the last manPPSeq. The needed commands to get there are calculated and added. Returns false if a manoeuvre solution is not found. (okVel is no longer supported)

References expandMan(), MAX_PP_MAN_CNT, p2p, and p2pCnt.

Referenced by UAvoidPath2::extendManoeuvre(), getP2PCnt(), testManSeq(), and testManSeqDeviation().

bool UManSeq::addManDriveon	(	UPoseV	fromPose,
		UPoseV	toPose,
		double	turnRad
	)

add manoeuvre using an estimate of the drive or driveon command, using the angle and cross distance gains as giudeline for turn radius.

Parameters

fromPose	is current pose.
toPose	is desired destination pose (line)
turnRad	is the turn radius to use.

Returns: true if the desired pose is reached and false if the line is reached after the desired end pose (the manoeuvre is then the best efford to reach the end pose line as fast as possible.

References expandManDriveon(), MAX_PP_MAN_CNT, p2p, and p2pCnt.

Referenced by UAvoidPath2::extendManoeuvre(), getP2PCnt(), and UFunctionAvoid::handlePoseToPose().

UManPPSeq * UManSeq::addP2P ( )

Add a new (empty) UManPPSeq as a next pose-to-pose sequence. Intended if already calculated manoeuvres are transfreerd to this structure.

References MAX_PP_MAN_CNT, p2p, and p2pCnt.

Referenced by USmlTag::getManSeq(), and getNeededDist().

bool UManSeq::copy ( UManSeq * source )

bool UManSeq::expandAddManALA	(	UManPPSeq *	ppSeq,
		bool	last,
		double	maxAcc,
		double	maxTurnAcc,
		double *	okVel
	)

protected

Expand a pose to pose manoeuvre to a sequence of simple manoeuvres in 'ppSeq', that can be translated into drive commands. Returns true if successful. The 'maxAcc' is the normal lateral acceleration. The end speed is assumed to be a maximum, that will be reduced if the turn radius and maxTurnAcc requires lower speed. The 'maxTurnAcc' is the centrepetal acceleration allowed. This function calls itself recursively if needed. Expand a pose to pose manoeuvre to a sequence of simple manoeuvres in 'ppSeq', that can be translated into drive commands. Returns true if successful. The 'maxAcc' is the normal lateral acceleration. The end speed is assumed to be a maximum, that will be reduced if the turn radius and maxTurnAcc requires lower speed. The 'maxTurnAcc' is the centrepetal acceleration allowed. This function calls itself recursively if needed.

References UManPPSeq::add(), dist, UPose2pose::get2hereALA(), UManPPSeq::getEndPoseV(), UPose::getMapToPosePose(), UManPPSeq::getMaxManCnt(), getNeededDist(), getNewArc(), getNewLine(), getNewStop(), UManPPSeq::getSeqCnt(), UManPPSeq::getStartPoseV(), UPoseV::getVel(), UPose::h, limitToPi(), maxd(), mind(), UManPPSeq::setEndPoseV(), UManPPSeq::setStartPoseV(), UPoseV::setVel(), signofd(), UPose::x, and UPose::y.

Referenced by getNeededDist().

bool UManSeq::expandAddManLALA	(	UManPPSeq *	ppSeq,
		bool	last,
		double	maxAcc,
		double	maxTurnAcc,
		double	minTurnRad
	)

protected

Expand a pose to pose manoeuvre to a sequence of simple manoeuvres in 'ppSeq', that can be translated into drive commands. Returns true if successful. The 'maxAcc' is the normal lateral acceleration. The end speed is assumed to be a maximum, that will be reduced if the turn radius and maxTurnAcc requires lower speed. The 'maxTurnAcc' is the centrepetal acceleration allowed. This function calls itself recursively if needed.

References UManPPSeq::add(), dist, UPose2pose::get2hereLALA(), UManPPSeq::getEndPoseV(), UPose::getMapToPosePose(), UManPPSeq::getMaxManCnt(), getNewArc(), getNewLine(), UManPPSeq::getSeqCnt(), UManPPSeq::getStartPoseV(), UPoseV::getVel(), UPose::h, UPoseV::setVel(), UPose::x, and UPose::y.

Referenced by expandMan(), and getNeededDist().

bool UManSeq::expandMan	(	UPoseV	fromPose,
		UPoseV	toPose,
		bool	last,
		double	maxAcc,
		double	maxTurnAcc,
		double	minTurnRad,
		UManPPSeq *	ppSeq,
		double *	okVel
	)

Expand a pose to pose manoeuvre to a sequence of simple manoeuvres. Returns false if not successful (no path found, i.e. 2cm backwards. REturns initial velocity in 'okVel' if all is OK. Returns a suggestec speed in 'okVel' (> MIN_SPEED = 0.2) if initial velocity is too fast. The 'maxAcc' is the normal lateral acceleration. The end speed is assumed to be a maximum, that will be reduced if the turn radius and maxTurnAcc requires lower speed. The 'maxTurnAcc' is the centrepetal acceleration allowed. This function sets the from-to pose, calls the expandAddMan(...) and set final velocity, distance and time used.

References UManPPSeq::calcEndVel(), expandAddManLALA(), releaseAll(), UManPPSeq::setEndPoseV(), UManPPSeq::setEndVel(), UManPPSeq::setManDist(), UManPPSeq::setManTime(), and UManPPSeq::setStartPoseV().

Referenced by addMan(), getNeededDist(), replaceMan(), and UResSmrCtl::sendNewManoeuvreToSMR().

bool UManSeq::expandManDriveon	(	UPoseV	fromPose,
		UPoseV	toPose,
		double	turnRad,
		UManPPSeq *	ppSeq
	)

Expand to an estimated begaviour for a driveon command, separating the behaviour into a turn an optional straight part andother turn and potentially a straight path to the end pose.

Parameters

fromPose	is the current pose and velocity.
toPose	is the desired target pose (line) and velocity, used in the driveon command
turnRad	is the turn radius to use this givess with gA of 2.0 and gD 0.75 a turnradius og ~4m.
ppSeq	is the manoeuvre sequence, where the manoeuvres are to be added.

Returns: true, if the end pose can be reached with the manoeuvre, else the toPose should be reevaluated from the manoeuvres.

References UManPPSeq::add(), UManPPSeq::calcEndPose(), UManPPSeq::calcEndVel(), UManPPSeq::fprint(), UPose2pose::get2line(), UPose::getDistance(), UPose::getMapToPosePose(), getNewArc(), getNewLine(), UManPPSeq::getStartPoseV(), UPoseV::print(), releaseAll(), UManPPSeq::setEndPoseV(), UManPPSeq::setEndVel(), UManPPSeq::setManDist(), UManPPSeq::setManTime(), UManPPSeq::setStartPoseV(), UPoseV::vel, UPose::x, and UPose::y.

Referenced by addManDriveon(), getNeededDist(), and replaceManDriveon().

void UManSeq::fprint	(	FILE *	fd,
		const char *	prestr
	)

Print status for manoeuvre sequence

References UManPPSeq::fprint(), p2p, p2pCnt, reusableCnt(), and reuseCnt.

Referenced by getNeededDist(), and testManSeq().

void UManSeq::freeAll ( )

Free all manoeuvre elements (delete)

References p2p, p2pCnt, reuse, and reuseCnt.

Referenced by getDataType(), and ~UManSeq().

virtual const char* UManSeq::getDataType ( )

inlinevirtual

Get (end) type of this structure

Reimplemented from UDataBase.

References freeAll(), releaseAll(), releaseAllMan(), releaseLast(), and truncate().

double UManSeq::getDeviationFromDirect ( )

Get deviation from the direct line to target

References ULine::getDistanceSq(), UManPPSeq::getEndPos(), p2p, p2pCnt, and ULineSegment::setFromPoints().

Referenced by getNeededDist().

double UManSeq::getDistance ( )

Get total route distance

References dist, UManPPSeq::getManDist(), p2p, and p2pCnt.

Referenced by UAvoidPath2::findObstAvoidRoute(), getNeededDist(), and UResSmrCtl::sendNewManoeuvreToSMR().

double UManSeq::getDistanceFromEndPoseLine ( double * maxHeading )

Calculate the distance from the end-pose line to the point on the manoeuvre furthest away from the line. Only the start/end points of each manoeuvere is used, so if one of the pose to pose manoeuvres has an almost full circle, then the circle is not counted.

Parameters

maxHeading is - if not NULL - set to the most different heading, relative to the end pose. a positive angle is to the left relative to the end pose heading.

Returns: the distance signed, where positive is to the left seen from end-pose pose.

References dist, U2Dlined::distanceSigned(), getEndPoseV(), UManPPSeq::getStartPoseV(), UPose::h, limitToPi(), p2p, p2pCnt, U2Dlined::setPH(), UPose::x, and UPose::y.

Referenced by getNeededDist(), UResSmrCtl::sendNewManoeuvreToSMR(), and testManSeqDeviation().

double UManSeq::getDistanceXYSigned	(	UPosition	pos,
		int *	where,
		bool	posIsRight,
		double	maxDist,
		UPose *	pHit,
		int *	idx,
		double *	t
	)

Get distance to this position from the manoeuvre lines. Positive is to the right of segment - seen from first end in direction towards other end (if 'posIsRight' is true). Returnes distance to either end or to a point on line whatever is closest. If 'where' is not NULL, then the closest part is returned here as: 0=point on line, 1= first point, 2= other end, 3 = too far away ( > maxDist). If 'where' is here (== 0), then 'pHit' is set to the closest position to the path. 'idx' is set to the manouvre sequence with the hit, else -1 if closest point is beyond last point. 't' is the distance into that sequence. If where != 0, then pHit will be unchanged

References UManPPSeq::getDistanceXYSigned(), UManPPSeq::getEndPoseV(), UManPPSeq::getManDist(), UManPPSeq::getStartPoseV(), p2p, p2pCnt, UPosition::x, UPose::x, UPosition::y, and UPose::y.

Referenced by getNeededDist(), and testManSeq().

UPoseV UManSeq::getEndPoseV ( )

Get the end pose as notet in manoeuver sequence.

References UPose::clear(), UManPPSeq::getEndPoseV(), p2p, p2pCnt, and UPoseV::setVel().

Referenced by UAvoidPath2::convertToManSeq(), getDistanceFromEndPoseLine(), getNeededDist(), getPoseV(), UResSmrCtl::sendNewManoeuvreToSMR(), testManSeq(), and testManSeqDeviation().

double UManSeq::getMaxTurnArc ( )

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

References a, UManPPSeq::getMaxTurnArc(), p2p, and p2pCnt.

Referenced by getNeededDist(), UResAvoid::methodCall(), and UResAvoid::methodCallV().

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

Get minimum distance to this line segment from the manoeuvre lines. Positive is to the right of segment - seen from first end in direction towards other end (if 'posIsRight' is true). Returnes distance to either end or to a point on line whatever is closest. If 'whereOnMan' is not NULL, then the closest part is returned here as: 0=point on line, 1= first point, 2= other end, 3 = too far away ( > maxDist). If 'whereOnSeg' is not NULL, then the closest part of segment is returned here as: 0=point on line, 1= first point, 2= other end. if 'poseOnMan' is not NULL then the closest pose is returned. if 'posOnSeg' is not NULL then closest position on segment 'seg' is returned. 'idx' is set to the manouvre sequence with the hit, else -1 if closest point is beyond last point.

References UManPPSeq::getEndPoseV(), UManPPSeq::getManDist(), UManPPSeq::getMinDistanceXYSigned(), UManPPSeq::getStartPoseV(), ULineSegment::length, p2p, p2pCnt, ULine::pos, UPosition::x, UPose::x, UPosition::y, and UPose::y.

Referenced by getNeededDist().

double UManSeq::getNeededAcc	(	double	startVel,
		double	endVel,
		double	distance
	)

inline

Calculate needed acceleration to get from speed to an end speed using only this distance. uses the formular V� - V_0� = 2 a s

References sqr().

double UManSeq::getNeededDist	(	double	startVel,
		double	endVel,
		double	acc
	)

inline

Calculate needed distance to accelerate from speed to an end speed using this accelertion. uses the formular V� - V_0� = 2 a s

References addP2P(), copy(), expandAddManALA(), expandAddManLALA(), expandMan(), expandManDriveon(), fprint(), getDeviationFromDirect(), getDistance(), getDistanceFromEndPoseLine(), getDistanceXYSigned(), getEndPoseV(), getMaxTurnArc(), getMinDistanceXYSigned(), getPoseV(), getStartPoseV(), getTime(), isValid(), print(), release(), removeLastArc(), requestObj(), reusableCnt(), and sqr().

Referenced by expandAddManALA().

UManoeuvre * UManSeq::getNewArc	(	double	radius,
		double	angle,
		bool	left,
		double	acceleration,
		double	targetVel
	)

Make a new manoeuvre from these data. An arc with this acceleration. Radius is turn radius in meter, sign is always positive. Angle is turn angle in radians, positive is left (counter-clockwise). Angle may be more than PI. Acceleration is signed, positive is increased speed forward. Returns a pointer to the created object.

References UManoeuvre::MAN_ARC, requestObj(), UManoeuvre::setAcc(), UManArc::setTurnAngle(), UManArc::setTurnRadius(), and UManoeuvre::setVel().

Referenced by copy(), expandAddManALA(), expandAddManLALA(), expandManDriveon(), USmlTag::getManSeq(), getNewArc(), and getP2PCnt().

UManoeuvre* UManSeq::getNewArc	(	double	radius,
		double	angle,
		double	acceleration,
		double	targetVel
	)

inline

Get a new arc amnoeuvre, where left or right is determined by angle sign. Else same as other function by same name.

References getNewArc(), and getNewStop().

UManoeuvre * UManSeq::getNewLine	(	double	dist,
		double	acceleration,
		double	targetVel
	)

Make a new manoeuvre from these data. A straight line with this acceleration. Distance is traveled distance in current direction. Acceleration is signed, positive is increased speed forward. Returns a pointer to the created object.

References UManoeuvre::MAN_LINE, requestObj(), UManoeuvre::setAcc(), UManLine::setDistance(), and UManoeuvre::setVel().

Referenced by copy(), expandAddManALA(), expandAddManLALA(), expandManDriveon(), USmlTag::getManSeq(), and getP2PCnt().

UManoeuvre * UManSeq::getNewStop ( )

Make a new stop manoeuvre. should set wheel speed to 0,0 at this point - may be an emergency stop, or just target reached. Returns a pointer to the created object.

References UManoeuvre::MAN_STOP, requestObj(), UManoeuvre::setAcc(), and UManoeuvre::setVel().

Referenced by copy(), expandAddManALA(), USmlTag::getManSeq(), and getNewArc().

UManPPSeq * UManSeq::getP2P ( int idx )

Get manoeuvre with this index

References p2p, and p2pCnt.

Referenced by copy(), UAvoidPath2::extendManoeuvre(), getP2PCnt(), UFunctionRoadDrive::handleCommand(), UFunctionDrivePos::handleDrivePos(), UPaintManoeuvre::makeManCloud(), UNavPaint::paintManData(), UClientFuncLaserGui::paintPathData(), UResSmrCtl::run(), UFunctionAvoid::sendCurrentAvoidPath(), UFunctionAvoid::sendCurrentPath(), UFunctionAvoid::sendManSeq(), and UResSmrCtl::sendNewManoeuvreToSMR().

int UManSeq::getP2PCnt ( )

inline

UPoseV UManSeq::getPoseV ( double atManTime )

Get pose at this time. Time is in seconds into the manoeuvre from its start pose.

References getEndPoseV(), UManPPSeq::getManTime(), p2p, and p2pCnt.

Referenced by getNeededDist().

UPoseV UManSeq::getStartPoseV ( )

Get the start pose as stored in this manoeuvre sequence.

References UManPPSeq::getStartPoseV(), p2p, and p2pCnt.

Referenced by getNeededDist(), UClientFuncLaser::handlePath(), UPaintManoeuvre::makeManCloud(), and UResSmrCtl::sendNewManoeuvreToSMR().

double UManSeq::getTime ( )

Get time to complete manoeuvre to end

References UManPPSeq::getManTime(), p2p, and p2pCnt.

Referenced by UAvoidPath2::getManTime(), and getNeededDist().

bool UManSeq::isValid ( )

Is manoeuvre sequence valid, i.e. atr there any movement defined

References UManPPSeq::isValid(), p2p, and p2pCnt.

Referenced by getNeededDist().

const char * UManSeq::print	(	const char *	prestr,
		char *	buff,
		const int	buffCnt
	)

Print manoeuvre status to string

References p2p, p2pCnt, UManPPSeq::print(), reusableCnt(), and reuseCnt.

Referenced by getNeededDist(), UResAvoid::methodCall(), UResAvoid::methodCallV(), UResSmrCtl::sendNewManoeuvreToSMR(), testManSeq(), and testManSeqDeviation().

void UManSeq::release ( UManoeuvre * toRelease )

protected

Release a manoeuvre object for reuse

References MAX_REUSABLE_LINE_OBJ, reuse, and reuseCnt.

Referenced by getNeededDist(), releaseAll(), removeLastArc(), and truncate().

void UManSeq::releaseAll ( UManPPSeq * ppseq )

Release all manoeuvres to the reuse stack from thei pose-to-pose sequence

References UManPPSeq::extract(), UManPPSeq::getSeqCnt(), release(), UManPPSeq::setFirstFixed(), and UManPPSeq::setLastFixed().

Referenced by expandMan(), expandManDriveon(), getDataType(), releaseAllMan(), and releaseLast().

void UManSeq::releaseAllMan ( )

Release all manoeuvres from all pose-to-pose sequences. Returns true if is moved to reuse stack. If not space in reuse stack, then deleted altogether.

References p2p, p2pCnt, and releaseAll().

Referenced by UAvoidPath2::clear(), copy(), UAvoidPath2::findObstAvoidRoute(), getDataType(), UClientManSeq::setFromTag(), testManSeq(), testManSeqDeviation(), and ~UManSeq().

void UManSeq::releaseLast ( )

Release last of the pose to pose manoeuvres, reducing the count of pp manoeuvres by 1.

References p2p, p2pCnt, and releaseAll().

Referenced by UAvoidPath2::convertToManSeq(), and getDataType().

void UManSeq::removeLastArc ( )

REmove last arc (probably unusable due to too small turn radius)

References UManPPSeq::getMan(), UManoeuvre::getManType(), UManPPSeq::getSeqCnt(), UManoeuvre::MAN_ARC, p2p, p2pCnt, release(), and UManPPSeq::truncate().

Referenced by getNeededDist().

bool UManSeq::replaceMan	(	UPoseV	midPose,
		bool	fixate,
		double	maxAcc,
		double	maxTurnAcc,
		double	minTurnRad,
		int	fromIdx,
		int	toIdx,
		bool	newEndpoint,
		double *	okVel
	)

Replace the manoeuvre set including fromIdx and toIdx (and any inbetween) with two manoeuvre sets using this intermediate pose If 'fixate' is true, then the new mid-pose is to be fixated, i.e. not to be replaced if route is near an obstacle close by, typically when passing a narrow gate. If 'newEndpoint' == true, then only one new sequence is generated. Returns true if all is OK. (okVEl not supported: Returns a suggested initian velocity in 'okVel' (>MIN_SPEED=0.2) if manoeuvres start with a emergency break), Returns 0.0 if no solution is found.

References expandMan(), UManPPSeq::getEndPoseV(), UManPPSeq::getEndVel(), UManPPSeq::getStartPoseV(), MAX_PP_MAN_CNT, maxi(), mini(), p2p, p2pCnt, UManPPSeq::setLastFixed(), and UPoseV::setVel().

Referenced by getP2PCnt().

bool UManSeq::replaceManDriveon	(	UPoseV	midPose,
		bool	fixate,
		double	turnRad,
		int	fromIdx,
		int	toIdx,
		bool	newEndpoint
	)

Replace some manoeuvres with a new new manoeuvres, that passes a new mid-point.

Parameters

midPose	is the new midPose.
fixate	if true, the poses will not be moved??
turnRad	is the turn radius to use
fromIdx	is the first manoeuvre index to be replaced.
toIdx	is the last index to be replaced.
newEndPoint	if true, then the new manoeuvre should end at the midPose, replacing the old manoeuvres.

Returns: true if the last of the inserted manoeuvres end at the desired end pose. returns false if the end pose line is reached later than desired.

References UManPPSeq::calcEndPose(), expandManDriveon(), UManPPSeq::getEndPoseV(), UManPPSeq::getEndVel(), UManPPSeq::getStartPoseV(), MAX_PP_MAN_CNT, maxi(), mini(), p2p, p2pCnt, UManPPSeq::setEndPoseV(), UManPPSeq::setLastFixed(), and UPoseV::setVel().