#include <unavview.h>

Public Member Functions
uint32_t	getColor (char col)

void	moveViewPose (double panAngle, bool reset, UPosition camResetPos, UPosition camResetFocus)

void	paint ()

bool	paintVarAdd (const char *name, bool add)

void	printRefSystems ()

bool	saveAsPng (const char *filename)

void	setRefSystemsHere ()

bool	setResource (UResBase *resource, bool remove)

void	setViewer (pcl::visualization::PCLVisualizer *newViewer, double camPar[])

	UNavView ()

	~UNavView ()

Public Attributes
int	cooSysLast
	last used coordinate system More...

bool	firstTextScannumber
	first time paint requires no remove from viewer More...

bool	followRobot
	follow robot in viewer More...

UPoseTime	lastCamFocus
	camera focus point at last paint More...

bool	paintBold

bool	paintCam

bool	paintCurves

bool	paintGmk

bool	paintGPS

bool	paintGridOdo

UPose	paintGridPose

double	paintGridSize [2]

bool	paintIntervalLines

bool	paintMapPose
	paint map-pose at bottom ov navigation display - if map pose plugin is available More...

int	paintObstCnt

bool	paintOdoPose
	paint odo-pose at bottom ov navigation display - if odo pose plugin is available More...

int	paintPathLinesCnt

bool	paintPathMidPoses

bool	paintPathSupportLines

UPaintPcp **	paintPcls
	point cloud update status More...

int	paintPclsMaxCnt
	number of paintpcps available More...

char	paintPcpHide [maxStrLng]

char	paintPcpShow [maxStrLng]

bool	paintPoly

char	paintPolyHide [maxStrLng]

int	paintPolyNameCnt

char	paintPolyShow [maxStrLng]

int	paintPoseHistCnt

int	paintPoseHistVecCnt

int	paintPoseHistVecLng

int	paintPoseRef

int	paintRoadAll

int	paintRoadHistCnt

int	paintRobot

int	paintScanHistCnt

UVarPool *	paintStructs [PAINT_MAX_STRUCTS]

int	paintStructsCnt

bool	paintUtmPose
	paint utm-pose at bottom ov navigation display - if utm pose plugin is available More...

bool	paintVar

int	paintVisPolyCnt

int	rangeRingCnt
	number of range rings to paint. More...

UPose	robotPose

UPose	robotPoseLast
	Pose of robot at last paint (and in last used coordinate system) More...

Static Public Attributes
static const int	maxStrLng = 100

static const int	PAINT_MAX_STRUCTS = 30

Protected Member Functions
UPose	getSystemOrigin (int myOrigin, bool *convertNeeded)

void	paintCams (UPoseTime seenFromPose)

void	paintCross (UImage *img, int x, int y, int size, CvScalar col, int lineWidth)

void	paintFeatures (UFeaturePool *featurePool, UPoseTime seenFromPose)

void	paintFreePoly (UProbPoly *poly, UPose seenFromPose, bool historic)

void	paintGmks (UPoseTime seenFromPose)

bool	paintManData (UClientManSeq *man, int num, UPoseTime seenFromPose)

void	paintObstGrp (UObstacleGroup *obst, UPose seenFromPose, int grpIdx)

void	paintOdoDataText ()

void	paintOdoGrid (UPose seenFromPose, double stepSize, double lastStepSize, bool bold)

void	paintPis (ULaserDataSet *scan, UPose seenFromPose, const int cnt)

void	paintPointClouds (UResPoseHist *posesys)

void	paintPolyItems (UPoseTime seenFromPose)

void	paintPose (UImage *img, int x, int y, double h, int size, CvScalar col, int lineWidth)

void	paintPoseHistLine (UPoseTime seenFromPose, UResPoseHist *path, int colidx, bool convert, UPose systemOrigin)

void	paintPoseHistLines (UPoseTime seenFromPose)

bool	paintRangeRings (UPosRot *sensorPos, int ringCnt)

void	paintRoadLine (URoadLineData *road, UPoseTime seenFromPose, bool inGray)

void	paintScansNewest (UPaintBase scanClouds, ULaserDataHistory scans)

bool	paintScanStatData (ULaserDataSet *scan, bool paintVar, bool paintVarL, bool paintTilt)

bool	paintVarDataText (bool paintAll)

Protected Attributes
UResCamIfCam *	camCam

UResCamIfGmk *	camGmk

UResCamIfPath *	camPath

double *	cmP

UImagePool *	imgPool

UPaintBase *	laserClouds

ULaserDataHistory *	laserData

UResLaserIfSf *	laserFeatures

UPaintManoeuvres *	manoeuvreClouds

UResNavIfMan *	navMan

UObstacleHist *	obsts

UPaintPolygons *	polyClouds

UPaintBase *	poseHist [PAINT_POSE_HIST_CNT]

UResPoseHist *	poseMap

UResPoseHist *	poseOdo

UPose	poseToRef [MAX_REF_SYS][MAX_REF_SYS]

UResPoseHist *	poseUtm

UResLaserIfScan *	resLaserData

UResLaserIfObst *	resObst

UResPcp *	resPcp

UResPoly *	resPoly

UResLaserIfRoad *	road

UPaintBase *	robot

UResVarPool *	varRoot

pcl::visualization::PCLVisualizer *	viewer

Static Protected Attributes
static const int	MAX_REF_SYS = 3

static const int	PAINT_POSE_HIST_CNT = 3
	number of pose hists More...

Detailed Description

Class to paint the navigation image

    @author Christian Andersen <jca@oersted.dtu.dk>

Constructor & Destructor Documentation

UNavView::UNavView ( )

Constructor

UNavView::~UNavView ( )

Destructor

References viewer.

Member Function Documentation

uint32_t UNavView::getColor ( char col )

Get a color value from a color character - mostly like color in scilab/matlab, i.e. 'r' is red, 'p' is pink

UPose UNavView::getSystemOrigin	(	int	myOrigin,
		bool *	convertNeeded
	)

protected

Get the origin of my coordinate system in the current paint coordinate syste. if coordinate systems are not the same, then the optional parameter 'convertNeeded' is set to true.

Parameters

myOrigin	is one of three values 0=odometry, 1=UTM, 2=map coordinates.
convertNeeded	is set to false, if current paint coordinates are the same as myOrigin.

Returns: the position of 'myOrigin' in current paint coordinate system.

References UPose::clear().

void UNavView::moveViewPose	(	double	panAngle,
		bool	reset,
		UPosition *	camResetPos,
		UPosition *	camResetFocus
	)

Set view angle relative to robot

Parameters

panAngle	is view angle relative to robot heading
reset	if true, then camera is moved to camResetPos relative to robot
camResetPos	is a position relative to robot were the camera is placed - may be NULL when not reset
camResetFocus	is the new focus position - relative to robot

References dist, viewer, UPosition::x, UPosition::y, and UPosition::z.

Referenced by UFuncView::run().

void UNavView::paint ( )

Paint laser data to this image-pool image

References absd(), UPose::clear(), UPose::getDistance(), UResPoseHist::getNewest(), ULaserDataSet::getPisCnt(), UPose::getPos(), UResPoseHist::getPoseAtTime(), UPose::getPoseToMap(), ULaserDataSet::getSensorPose(), ULaserDataSet::getSerial(), ULaserDataSet::isStatValid(), mini(), scan, UPosRot::set(), UPosition::set(), UPoseTime::t, viewer, UPosition::x, UPose::x, UPosition::y, UPose::y, and UPosition::z.

Referenced by UFuncView::run().

void UNavView::paintCams ( UPoseTime seenFromPose )

protected

Paint known camera positions

void UNavView::paintCross	(	UImage *	img,
		int	x,
		int	y,
		int	size,
		CvScalar	col,
		int	lineWidth
	)

protected

Paint a cross symbol at this position

void UNavView::paintFeatures	(	UFeaturePool *	featurePool,
		UPoseTime	seenFromPose
	)

protected

Paint simple features from a feature pool

References UFeatureData::getDataType(), UPose::getMapToPose(), ULineSegment::getOtherEnd(), UPose::getPoseToMap(), UFeaturePool::getScan(), UFeaturePool::getScansCnt(), UFeatureData::getSegs(), UFeatureData::getSegsCnt(), UFeatureData::getSegsInt(), ULine::pos, UFeatureData::segsStr, viewer, UPosition::x, UPosition::y, and UPosition::z.

void UNavView::paintFreePoly	(	UProbPoly *	poly,
		UPose	seenFromPose,
		bool	historic
	)

protected

Paint a vision based polygon

References UProbPoly::getIsObst(), UPose::getMapToPose(), UPolygon::getPoints(), UPolygon::getPointsCnt(), viewer, UPosition::x, UPosition::y, and UPosition::z.

void UNavView::paintGmks ( UPoseTime seenFromPose )

protected

Paint known GMK positions

bool UNavView::paintManData	(	UClientManSeq *	man,
		int	num,
		UPoseTime	seenFromPose
	)

protected

Paint one manoeuver sequence 'man', in res if the best, else in weak colors dependent in the 'num' parameter

void UNavView::paintObstGrp	(	UObstacleGroup *	obst,
		UPose	seenFromPose,
		int	grpIdx
	)

protected

Paint obstacles in this obstacle group

References UPose::getMapToPose(), UObstacleGroup::getObstacle(), UObstacleGroup::getObstsCnt(), UPolygon::getPoint(), UPolygon::getPointsCnt(), UPose::getPoseToMap(), UObstacle::isValid(), viewer, UPosition::x, UPosition::y, and UPosition::z.

void UNavView::paintOdoDataText ( )

protected

Paint current odometry position and time as text in bottom left corner

References UTime::clear(), UTime::getDateString(), UTime::getDecSec(), UTime::getMicrosec(), UResPoseHist::getPoseAtTime(), UTime::getSec(), UTime::getTimeAsString(), UPose::h, UTime::Now(), UPoseTVQ::q, UPoseTime::t, UPoseTVQ::vel, viewer, UPose::x, and UPose::y.

void UNavView::paintOdoGrid	(	UPose	seenFromPose,
		double	stepSize,
		double	lastStepSize,
		bool	bold
	)

protected

Paint odometry reference grid around this pose (and remove old - if any)

References a, roundi(), UPosition::set(), viewer, UPosition::x, UPose::x, UPosition::y, and UPose::y.

void UNavView::paintPis	(	ULaserDataSet *	scan,
		UPose	seenFromPose,
		const int	cnt
	)

protected

Paint passable intervals fount in this scan

References UClientLaserPi::getLeftPos(), UPose::getMapToPose(), ULaserDataSet::getPis(), ULaserDataSet::getPisCnt(), UClientLaserPi::getRightPos(), viewer, UPosition::x, UPosition::y, and UPosition::z.

void UNavView::paintPointClouds ( UResPoseHist * posesys )

protected

Paint all Pointclouds in point cloud pool (PCP)

Parameters

posesys is the pose history for the current coordinate system

References UPaintBase::bold, UPaintBase::getCooSysView(), UResPoseHist::getPoseAtTime(), UPaintPcp::pcl, UPcpItem::updateTime, and viewer.

void UNavView::paintPolyItems ( UPoseTime seenFromPose )

protected

paint the lines in the polyItem module

References UPolygon::color, UPolyItem::cooSys, UPolygon::getPointsCnt(), UPolyItem::name, pattern_match(), and viewer.

void UNavView::paintPose	(	UImage *	img,
		int	x,
		int	y,
		double	h,
		int	size,
		CvScalar	col,
		int	lineWidth
	)

protected

Paint a simple robot outline Paint a simple robot outline Paint a simple robot outline Paint a simple robot outline Paint a simple robot outline Paint a pose-symbol at this pixel position

void UNavView::paintPoseHistLine	(	UPoseTime	seenFromPose,
		UResPoseHist *	path,
		int	colidx,
		bool	convert,
		UPose	systemOrigin
	)

protected

Paint historic scans Paint pose history line

Parameters

seenFromPose	current pose in current coordinate system
path	system path to be painted
colidx	index number to paint colour (pt. 0=red, 1=cyan, 2=magenta)
convert	should the path coordinates be converted to local system (true if conversion is needed)
systemOrigin	origin of path coordinate system in current system.

void UNavView::paintPoseHistLines ( UPoseTime seenFromPose )

protected

Paint path lines for all available reference systems

References viewer.

bool UNavView::paintRangeRings	(	UPosRot *	sensorPos,
		int	ringCnt
	)

protected

Paint laser scanner range rings

Parameters

sensorPos	is the 6d sensor pose relative to the robot.
ringCnt	is the number of range rings to paint.

void UNavView::paintRoadLine	(	URoadLineData *	road,
		UPoseTime	seenFromPose,
		bool	inGray
	)

protected

Paint road line, as line segment and poly-line

void UNavView::paintScansNewest	(	UPaintBase *	scanClouds,
		ULaserDataHistory *	scans
	)

protected

Paint scans so that scanCount are painted, newest scan are painted in bold

Parameters

scanClouds	the scanners point existing clouds
scans	are the raw scanner data (newest)

References UPaintLaserScans::addScan(), UPaintLaserScans::ageNewestScan(), UPaintBase::bold, UPaintLaserScans::cloudsCnt, UPaintBase::getCooSysView(), UPaintLaserScans::getNewestSerial(), ULaserDataSet::getPose(), UResPoseHist::getPoseAtTime(), ULaserDataSet::getScanTime(), ULaserDataSet::getSerial(), mini(), UPaintLaserScans::remove(), UPaintLaserScans::removeAll(), scan, UPaintBase::setViewCooSys(), UPaintLaserScans::unPaintAll(), and viewer.

bool UNavView::paintScanStatData	(	ULaserDataSet *	scan,
		bool	paintVar,
		bool	paintVarL,
		bool	paintTilt
	)

protected

Paint scan line-fit variance curves (and other curves) in top-right corner

bool UNavView::paintVarAdd	(	const char *	name,
		bool	add
	)

Add or remove paint var structures

References UVarPool::getStructDeep().

Referenced by UFuncView::handleCommand().

bool UNavView::paintVarDataText ( bool paintAll )

protected

Paint variables and values in top-left corner (and down)

void UNavView::printRefSystems ( )

Print the coordinate references to console for all available conversions

References UTime::getDateTimeAsString(), UTime::getMicrosec(), UTime::getSec(), UPose::h, UPose::x, and UPose::y.

bool UNavView::saveAsPng ( const char * filename )

Save viewed image to a png-file

Parameters

filename is the name to use

Returns: true if saved

void UNavView::setRefSystemsHere ( )

Set all coordinaate systems to the same spot (this)

References UPose::clear(), UResPoseHist::clear(), UPose::getMapToPosePose(), UResPoseHist::getPoseAtTime(), and UPose::getPoseToMapPose().

Referenced by UFuncView::handleCommand(), and UFuncView::run().

bool UNavView::setResource	(	UResBase *	resource,
		bool	remove
	)

Set ressource as needed (probably not used by this resource)

References UResPoseHist::getMapPoseID(), UResPoseHist::getOdoPoseID(), UResLaserIfObst::getResClassID(), UResCamIfPath::getResClassID(), UResCamIfCam::getResClassID(), UResCamIfGmk::getResClassID(), UImagePool::getResClassID(), UResLaserIfSf::getResClassID(), UResVarPool::getResClassID(), UResLaserIfScan::getResClassID(), UResLaserIfRoad::getResClassID(), UResNavIfMan::getResClassID(), UResPoseHist::getUtmPoseID(), and UResBase::isA().

Referenced by UFuncView::setResource().

void UNavView::setViewer	(	pcl::visualization::PCLVisualizer *	newViewer,
		double	camPar[]
	)

inline

Set the viewer to show the data This call is not thread safe, so it should be set before first paint call.

Parameters

newViewer must be an instanciated viewer.

References UPaintBase::bold, img, scan, and viewer.

Referenced by UFuncView::run().

Member Data Documentation

UResCamIfCam* UNavView::camCam

protected

Pointer to pool of camera details

UResCamIfGmk* UNavView::camGmk

protected

Pointer to guidemark pool

UResCamIfPath* UNavView::camPath

protected

Pointer to vision based path

double* UNavView::cmP

protected

Viewer to show data

int UNavView::cooSysLast

last used coordinate system

bool UNavView::firstTextScannumber

first time paint requires no remove from viewer

bool UNavView::followRobot

follow robot in viewer

Referenced by UFuncView::handleCommand(), and UFuncView::run().

UImagePool* UNavView::imgPool

protected

Image pool pointer

UPaintBase* UNavView::laserClouds

protected

Laser scans from one laser scanner

ULaserDataHistory* UNavView::laserData

protected

Pointer to copy of basic laser scanner data

UResLaserIfSf* UNavView::laserFeatures

protected

Pool with simple features - as lines and points

UPoseTime UNavView::lastCamFocus

camera focus point at last paint

UPaintManoeuvres* UNavView::manoeuvreClouds

protected

Manoeuvre data

const int UNavView::MAX_REF_SYS = 3

staticprotected

Maximum map reference systems (currently 3: odo(0), utm(1), map(2))

const int UNavView::maxStrLng = 100

static

Referenced by UFuncView::handleCommand().

UResNavIfMan* UNavView::navMan

protected

Navigation path resource from navigation server

UObstacleHist* UNavView::obsts

protected

Pointer to obstacle data

const int UNavView::PAINT_MAX_STRUCTS = 30

static

Max number of painted variable structures

const int UNavView::PAINT_POSE_HIST_CNT = 3

staticprotected

number of pose hists

bool UNavView::paintBold

Paint in bold (thicker bigger)

Referenced by UFuncView::paintBold().

bool UNavView::paintCam

Paint camera position

Referenced by UFuncView::handleCommand().

bool UNavView::paintCurves

Paint statistical curves

Referenced by UFuncView::handleCommand().

bool UNavView::paintGmk

Paint guidemarks

Referenced by UFuncView::handleCommand().

bool UNavView::paintGPS

Paint EKF (GPS) data

bool UNavView::paintGridOdo

paint grid (rectangular odo grid

Referenced by UFuncView::handleCommand().

UPose UNavView::paintGridPose

Position around the last odometry grid

double UNavView::paintGridSize[2]

Paint vision based polygon Distance between grid lines - in meter

Referenced by UFuncView::handleCommand().

bool UNavView::paintIntervalLines

Paint interval lines

Referenced by UFuncView::handleCommand().

bool UNavView::paintMapPose

paint map-pose at bottom ov navigation display - if map pose plugin is available

Referenced by UFuncView::handleCommand().

int UNavView::paintObstCnt

Paint planning info (var and mission line) Number of obstacle groups (max) to paint

Referenced by UFuncView::handleCommand().

bool UNavView::paintOdoPose

paint odo-pose at bottom ov navigation display - if odo pose plugin is available

Referenced by UFuncView::handleCommand().

int UNavView::paintPathLinesCnt

Paint path lines - alternative and used path

Referenced by UFuncView::handleCommand().

bool UNavView::paintPathMidPoses

Paint tested (mid) poses for generation of this path

Referenced by UFuncView::handleCommand().

bool UNavView::paintPathSupportLines

Paint tangent lines and no-visibility lines

UPaintPcp** UNavView::paintPcls

point cloud update status

int UNavView::paintPclsMaxCnt

number of paintpcps available

char UNavView::paintPcpHide[maxStrLng]

hide these pcps (default hide none)

Referenced by UFuncView::handleCommand().

char UNavView::paintPcpShow[maxStrLng]

but show these these pcps among the hidden (default show all)

Referenced by UFuncView::handleCommand().

bool UNavView::paintPoly

Paint poly items - planned missions etc.

Referenced by UFuncView::handleCommand().

char UNavView::paintPolyHide[maxStrLng]

hide these polygons (default hide none)

Referenced by UFuncView::handleCommand().

int UNavView::paintPolyNameCnt

Paint name of poly items - at maximum this number of chars.

Referenced by UFuncView::handleCommand().

char UNavView::paintPolyShow[maxStrLng]

but show these these polygons among the hidden (default show all)

Referenced by UFuncView::handleCommand().

int UNavView::paintPoseHistCnt

Paint path lines for all, also alternaive paths Number of poses in pose history to paint

Referenced by UFuncView::handleCommand().

int UNavView::paintPoseHistVecCnt

Pose interval for for every heading vector paint (0 is not)

Referenced by UFuncView::handleCommand().

int UNavView::paintPoseHistVecLng

Pose heading vector length in pixels

Referenced by UFuncView::handleCommand().

int UNavView::paintPoseRef

Used pose reference 0=odoPose, 1=utmPose 2= mapPose

Referenced by UFuncView::handleCommand().

int UNavView::paintRoadAll

Paint all road lines - not just the primary newest road

Referenced by UFuncView::handleCommand().

int UNavView::paintRoadHistCnt

Paint road lines, if 0 then no roads are painted

Referenced by UFuncView::handleCommand().

int UNavView::paintRobot

Paint robot as: 0=mmr, 1=smr, 2=hako

Referenced by UFuncView::setRobot().

int UNavView::paintScanHistCnt

Number of scans to use in path hist

Referenced by UFuncView::handleCommand().

UVarPool* UNavView::paintStructs[PAINT_MAX_STRUCTS]

Paint variables from these structures

int UNavView::paintStructsCnt

Number of used paint structures

bool UNavView::paintUtmPose

paint utm-pose at bottom ov navigation display - if utm pose plugin is available

Referenced by UFuncView::handleCommand().

bool UNavView::paintVar

Paint variables (in structures) - used to hide all

Referenced by UFuncView::handleCommand().

int UNavView::paintVisPolyCnt

Number of vision poygons to paint

Referenced by UFuncView::handleCommand().

UPaintPolygons* UNavView::polyClouds

protected

Polygons

UPaintBase* UNavView::poseHist[PAINT_POSE_HIST_CNT]

protected

Pose history

UResPoseHist* UNavView::poseMap

protected

Pointer to recorded pose history - in Map coordinates

UResPoseHist* UNavView::poseOdo

protected

Pointer to recorded pose history - in odometry coordinates

UPose UNavView::poseToRef[MAX_REF_SYS][MAX_REF_SYS]

protected

Pose reference conversion offset First index is source second index is destination system

UResPoseHist* UNavView::poseUtm

protected

Pointer to recorded pose history - in UTM coordinates

int UNavView::rangeRingCnt

number of range rings to paint.

Referenced by UFuncView::run(), and UFuncView::setRangeRingCnt().

UResLaserIfScan* UNavView::resLaserData

protected

Pointer to resource with laser scan data

UResLaserIfObst* UNavView::resObst

protected

Pointer to laser scanner obstacle resource

UResPcp* UNavView::resPcp

protected

Pointer to resource with laser scan data

UResPoly* UNavView::resPoly

protected

pointer to polyItems

UResLaserIfRoad* UNavView::road

protected

Pointer to road data

UPaintBase* UNavView::robot

protected

robots

UPose UNavView::robotPose

convert a position in one of the supported coordinate systems to a pixel position

Parameters

pos	is the position to converted
coordinateSystem	is the ordinal number of the coordinate system of pos, 0=odo, 1= utm, 2=map.
seenFromPose	is the current position of the robot in current coordinate system

Returns: pixel position - limited to max 10000 pixels away from display origin convert a position in one of the supported coordinate systems to a pixel position

Parameters

pos	is the position to converted
seenFromPose	is the current position of the robot in current coordinate system
systemOrigin	is the origin of the 'pos' position in the displayed coodinate system (returned by the getSystemOrigin(...) call)
convert	is a flag - if false no coordinate conversion is needed. (returned by the getSystemOrigin(...) call)

Returns: pixel position - limited to max 10000 pixels away from display origin Convert this position in displayed coordinate system to a pixel position

Parameters

pos	is the position (only x,y is used).

Returns: a position to be used in paint call. Convert this pose position in displayed coordinate system to a pixel position

Parameters

pose	is the position (only x,y is used).

Returns: a position to be used in paint call. Max range is the total height of the navigation window. Robot position on image (in screen coordinates - meters above bottom-center of screen)

Referenced by UFuncView::setRobotPose().

UPose UNavView::robotPoseLast

Pose of robot at last paint (and in last used coordinate system)

UResVarPool* UNavView::varRoot

protected

Pointer to (global) var-pool

pcl::visualization::PCLVisualizer* UNavView::viewer

protected

Viewer to show data

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

Public Member Functions

Public Attributes

Static Public Attributes

Protected Member Functions

Protected Attributes

Static Protected Attributes

Detailed Description

Constructor & Destructor Documentation

Member Function Documentation

Member Data Documentation