swri_route_util Namespace Reference

Classes
struct	DistanceReport
struct	ObstacleData
struct	PathPosition
class	Route
class	RoutePoint
struct	SpeedForCurvatureParameters
struct	SpeedForObstaclesParameters

Typedefs
typedef boost::shared_ptr< Route const >	RouteConstPtr
typedef boost::shared_ptr< Route >	RoutePtr

Functions
static void	addItem (mcm::KeyValueArray &m, const std::string &key, const std::string &value)
static double	estimateCurvature (const Route &route, const size_t index, double filter_size)
bool	extractSubroute (Route &sub_route, const Route &route, const marti_nav_msgs::RoutePosition &start, const marti_nav_msgs::RoutePosition &end)
void	fillOrientations (marti_nav_msgs::Path &path)
void	fillOrientations (marti_nav_msgs::Plan &path)
void	fillOrientations (Route &route, const tf::Vector3 &up=tf::Vector3(0.0, 0.0, 1.0))
bool	findLocalNearestDistanceForward (const marti_nav_msgs::Path &path, const double x, const double y, PathPosition &nearest_position, double &nearest_separation)
bool	findLocalNearestDistanceForward (const marti_nav_msgs::Plan &path, const double x, const double y, marti_nav_msgs::PlanPosition &nearest_position, double &nearest_separation)
void	generateObstacleData (std::vector< ObstacleData > &obstacle_data, const stu::Transform g_route_from_obs, const mnm::ObstacleArray &obstacles_msg)
void	generateObstacleData (std::vector< ObstacleData > &obstacle_data, const stu::Transform g_route_from_obs, const mnm::TrackedObjectArray &obstacles_msg)
void	generateObstacleData (std::vector< ObstacleData > &obstacle_data, const swri_transform_util::Transform g_route_from_obs, const marti_nav_msgs::ObstacleArray &obstacles_msg)
void	generateObstacleData (std::vector< ObstacleData > &obstacle_data, const swri_transform_util::Transform g_route_from_obs, const marti_nav_msgs::TrackedObjectArray &obstacles_msg)
void	getPathPose (const marti_nav_msgs::Path &path, const PathPosition position, tf::Transform &tf, const bool allow_extrapolation=false)
void	getPathPose (const marti_nav_msgs::Plan &path, const marti_nav_msgs::PlanPosition position, tf::Transform &tf, const bool allow_extrapolation=false)
void	getPathPosition (const marti_nav_msgs::Path &path, const PathPosition position, tf::Vector3 &tf)
void	getPlanPosition (const marti_nav_msgs::Plan &path, const marti_nav_msgs::PlanPosition position, tf::Vector3 &tf, bool extrapolate=false)
tf::Vector3	getPointPosition (const marti_nav_msgs::PlanPoint &pt)
static double	interpolateAngle (double from, double to, double t)
static double	interpolateAngle (double from, double to, double t)
void	interpolatePlanPosition (const marti_nav_msgs::Plan &path, const marti_nav_msgs::PlanPosition position, marti_nav_msgs::PlanPoint &pt, bool extrapolate=false)
bool	interpolateRoutePosition (RoutePoint &dst, const Route &route, const mnm::RoutePosition &position, bool allow_extrapolation)
bool	interpolateRoutePosition (RoutePoint &point, const Route &route, const marti_nav_msgs::RoutePosition &position, bool allow_extrapolation)
static void	interpolateRouteSegment (RoutePoint &dst, const RoutePoint &p0, const RoutePoint &p1, double distance)
static geometry_msgs::Point	makePoint (const double x, const double y)
void	markerForRouteSpeeds (visualization_msgs::Marker &marker, const Route &route, const marti_nav_msgs::RouteSpeedArray &speeds, double scale)
void	markerForRouteSpeeds (vm::Marker &m, const Route &route, const mnm::RouteSpeedArray &speeds, double scale)
static double	maxSpeedForCurvature (double curvature, const SpeedForCurvatureParameters &params)
static void	msgFromPoint (marti_nav_msgs::RoutePoint &dst, const RoutePoint &src)
static void	nearestDistanceToLineSegment (double &min_distance_from_line, double &min_distance_on_line, const tf::Vector3 &p0, const tf::Vector3 &p1, const tf::Vector3 &p, bool extrapolate_start, bool extrapolate_end)
static void	nearestDistanceToLineSegment (double &min_distance_from_line, double &min_distance_on_line, const tf::Vector3 &p0, const tf::Vector3 &p1, const tf::Vector3 &p, bool extrapolate_start, bool extrapolate_end)
static void	normalizePathPosition (const marti_nav_msgs::Path &path, PathPosition &position)
void	normalizePlanPosition (marti_nav_msgs::PlanPosition &position, const marti_nav_msgs::Plan &path)
bool	normalizeRoutePosition (marti_nav_msgs::RoutePosition &normalized_position, const Route &route, const marti_nav_msgs::RoutePosition &position)
bool	normalizeRoutePosition (mnm::RoutePosition &normalized_position, const Route &route, const mnm::RoutePosition &position)
bool	planDistance (double &distance, const marti_nav_msgs::PlanPosition &start, const marti_nav_msgs::PlanPosition &end, const marti_nav_msgs::Plan &route)
bool	planDistance (double &distance, const marti_nav_msgs::PlanPosition &start, const marti_nav_msgs::PlanPosition &end, const mnm::Plan &route)
static void	pointFromMsg (RoutePoint &dst, const marti_nav_msgs::RoutePoint &src)
bool	projectOntoPlan (marti_nav_msgs::PlanPosition &position, const marti_nav_msgs::Plan &route, const tf::Vector3 &point, bool extrapolate_before_start, bool extrapolate_past_end)
bool	projectOntoPlan (mnm::PlanPosition &position, const mnm::Plan &route, const tf::Vector3 &point, bool extrapolate_before_start, bool extrapolate_past_end)
bool	projectOntoPlanWindow (marti_nav_msgs::PlanPosition &position, const marti_nav_msgs::Plan &route, const tf::Vector3 &point, const marti_nav_msgs::PlanPosition &window_start, const marti_nav_msgs::PlanPosition &window_end)
bool	projectOntoRoute (marti_nav_msgs::RoutePosition &position, const Route &route, const tf::Vector3 &point, bool extrapolate_before_start, bool extrapolate_past_end)
bool	projectOntoRoute (mnm::RoutePosition &position, const Route &route, const tf::Vector3 &point, bool extrapolate_before_start, bool extrapolate_past_end)
bool	projectOntoRouteWindow (marti_nav_msgs::RoutePosition &position, const Route &route, const tf::Vector3 &point, const marti_nav_msgs::RoutePosition &window_start, const marti_nav_msgs::RoutePosition &window_end)
bool	projectOntoRouteWindow (mnm::RoutePosition &position, const Route &route, const tf::Vector3 &point, const mnm::RoutePosition &window_start, const mnm::RoutePosition &window_end)
void	projectToXY (marti_nav_msgs::Plan &route)
void	projectToXY (Route &route)
bool	routeDistance (double &distance, const marti_nav_msgs::RoutePosition &start, const marti_nav_msgs::RoutePosition &end, const Route &route)
bool	routeDistance (double &distance, const mnm::RoutePosition &start, const mnm::RoutePosition &end, const Route &route)
bool	routeDistances (std::vector< double > &distances, const marti_nav_msgs::RoutePosition &start, const std::vector< marti_nav_msgs::RoutePosition > &ends, const Route &route)
bool	routeDistances (std::vector< double > &distances, const mnm::RoutePosition &start, const std::vector< mnm::RoutePosition > &ends, const Route &route)
void	speedsForCurvature (marti_nav_msgs::RouteSpeedArray &speeds, const Route &route, const SpeedForCurvatureParameters &parameters)
void	speedsForCurvature (mnm::RouteSpeedArray &speeds, const Route &route, const SpeedForCurvatureParameters &parameters)
void	speedsForObstacles (marti_nav_msgs::RouteSpeedArray &speeds, std::vector< DistanceReport > &reports, const Route &route, const marti_nav_msgs::RoutePosition &route_position, const std::vector< ObstacleData > &obstacles, const SpeedForObstaclesParameters &parameters)
void	speedsForObstacles (mnm::RouteSpeedArray &speeds, std::vector< DistanceReport > &reports, const Route &route, const mnm::RoutePosition &route_position, const std::vector< ObstacleData > &obstacles, const SpeedForObstaclesParameters &p)
void	transform (marti_nav_msgs::Path &path, const swri_transform_util::Transform &transform, const std::string &target_frame)
void	transform (marti_nav_msgs::Plan &path, const swri_transform_util::Transform &transform, const std::string &target_frame)
void	transform (Route &route, const swri_transform_util::Transform &transform, const std::string &target_frame)

Typedef Documentation

RouteConstPtr

typedef boost::shared_ptr<Route const> swri_route_util::RouteConstPtr

Definition at line 171 of file route.h.

RoutePtr

typedef boost::shared_ptr<Route> swri_route_util::RoutePtr

Definition at line 170 of file route.h.

Function Documentation

addItem()

static void swri_route_util::addItem ( mcm::KeyValueArray &  m, const std::string &  key, const std::string &  value  )

static

Definition at line 45 of file route_speeds.cpp.

estimateCurvature()

static double swri_route_util::estimateCurvature ( const Route &  route, const size_t  index, double  filter_size  )

static

Definition at line 190 of file route_speeds.cpp.

extractSubroute()

bool swri_route_util::extractSubroute	(	Route &	sub_route,
		const Route &	route,
		const marti_nav_msgs::RoutePosition &	start,
		const marti_nav_msgs::RoutePosition &	end
	)

Definition at line 727 of file util.cpp.

fillOrientations() [1/3]

void swri_route_util::fillOrientations

(

marti_nav_msgs::Path &

path

)

Definition at line 59 of file path_util.cpp.

fillOrientations() [2/3]

void swri_route_util::fillOrientations

(

marti_nav_msgs::Plan &

path

)

Definition at line 68 of file plan_util.cpp.

fillOrientations() [3/3]

void swri_route_util::fillOrientations	(	Route &	route,
		const tf::Vector3 &	up = tf::Vector3(0.0, 0.0, 1.0)
	)

Definition at line 61 of file util.cpp.

findLocalNearestDistanceForward() [1/2]

bool swri_route_util::findLocalNearestDistanceForward	(	const marti_nav_msgs::Path &	path,
		const double	x,
		const double	y,
		PathPosition &	nearest_position,
		double &	nearest_separation
	)

Definition at line 90 of file path_util.cpp.

findLocalNearestDistanceForward() [2/2]

bool swri_route_util::findLocalNearestDistanceForward	(	const marti_nav_msgs::Plan &	path,
		const double	x,
		const double	y,
		marti_nav_msgs::PlanPosition &	nearest_position,
		double &	nearest_separation
	)

Definition at line 99 of file plan_util.cpp.

generateObstacleData() [1/4]

void swri_route_util::generateObstacleData	(	std::vector< ObstacleData > &	obstacle_data,
		const stu::Transform	g_route_from_obs,
		const mnm::ObstacleArray &	obstacles_msg
	)

Definition at line 291 of file route_speeds.cpp.

generateObstacleData() [2/4]

void swri_route_util::generateObstacleData	(	std::vector< ObstacleData > &	obstacle_data,
		const stu::Transform	g_route_from_obs,
		const mnm::TrackedObjectArray &	obstacles_msg
	)

Definition at line 328 of file route_speeds.cpp.

generateObstacleData() [3/4]

void swri_route_util::generateObstacleData	(	std::vector< ObstacleData > &	obstacle_data,
		const swri_transform_util::Transform	g_route_from_obs,
		const marti_nav_msgs::ObstacleArray &	obstacles_msg
	)

generateObstacleData() [4/4]

void swri_route_util::generateObstacleData	(	std::vector< ObstacleData > &	obstacle_data,
		const swri_transform_util::Transform	g_route_from_obs,
		const marti_nav_msgs::TrackedObjectArray &	obstacles_msg
	)

getPathPose() [1/2]

void swri_route_util::getPathPose	(	const marti_nav_msgs::Path &	path,
		const PathPosition	position,
		tf::Transform &	tf,
		const bool	allow_extrapolation = false
	)

Definition at line 253 of file path_util.cpp.

getPathPose() [2/2]

void swri_route_util::getPathPose	(	const marti_nav_msgs::Plan &	path,
		const marti_nav_msgs::PlanPosition	position,
		tf::Transform &	tf,
		const bool	allow_extrapolation = false
	)

getPathPosition()

void swri_route_util::getPathPosition	(	const marti_nav_msgs::Path &	path,
		const PathPosition	position,
		tf::Vector3 &	tf
	)

Definition at line 228 of file path_util.cpp.

getPlanPosition()

void swri_route_util::getPlanPosition	(	const marti_nav_msgs::Plan &	path,
		const marti_nav_msgs::PlanPosition	position,
		tf::Vector3 &	tf,
		bool	extrapolate = false
	)

Definition at line 568 of file plan_util.cpp.

getPointPosition()

tf::Vector3 swri_route_util::getPointPosition ( const marti_nav_msgs::PlanPoint &  pt )

inline

Definition at line 89 of file plan_util.h.

interpolateAngle() [1/2]

static double swri_route_util::interpolateAngle ( double  from, double  to, double  t  )

static

Definition at line 207 of file path_util.cpp.

interpolateAngle() [2/2]

static double swri_route_util::interpolateAngle ( double  from, double  to, double  t  )

static

Definition at line 216 of file plan_util.cpp.

interpolatePlanPosition()

void swri_route_util::interpolatePlanPosition	(	const marti_nav_msgs::Plan &	path,
		const marti_nav_msgs::PlanPosition	position,
		marti_nav_msgs::PlanPoint &	pt,
		bool	extrapolate = false
	)

Definition at line 535 of file plan_util.cpp.

interpolateRoutePosition() [1/2]

bool swri_route_util::interpolateRoutePosition	(	RoutePoint &	dst,
		const Route &	route,
		const mnm::RoutePosition &	position,
		bool	allow_extrapolation
	)

Definition at line 542 of file util.cpp.

interpolateRoutePosition() [2/2]

bool swri_route_util::interpolateRoutePosition	(	RoutePoint &	point,
		const Route &	route,
		const marti_nav_msgs::RoutePosition &	position,
		bool	allow_extrapolation
	)

interpolateRouteSegment()

static void swri_route_util::interpolateRouteSegment ( RoutePoint &  dst, const RoutePoint &  p0, const RoutePoint &  p1, double  distance  )

static

Definition at line 458 of file util.cpp.

makePoint()

static geometry_msgs::Point swri_route_util::makePoint ( const double  x, const double  y  )

static

Definition at line 38 of file visualization.cpp.

markerForRouteSpeeds() [1/2]

void swri_route_util::markerForRouteSpeeds	(	visualization_msgs::Marker &	marker,
		const Route &	route,
		const marti_nav_msgs::RouteSpeedArray &	speeds,
		double	scale
	)

markerForRouteSpeeds() [2/2]

void swri_route_util::markerForRouteSpeeds	(	vm::Marker &	m,
		const Route &	route,
		const mnm::RouteSpeedArray &	speeds,
		double	scale
	)

Definition at line 47 of file visualization.cpp.

maxSpeedForCurvature()

static double swri_route_util::maxSpeedForCurvature ( double  curvature, const SpeedForCurvatureParameters &  params  )

static

Definition at line 223 of file route_speeds.cpp.

msgFromPoint()

static void swri_route_util::msgFromPoint ( marti_nav_msgs::RoutePoint &  dst, const RoutePoint &  src  )

static

Definition at line 71 of file route.cpp.

nearestDistanceToLineSegment() [1/2]

static void swri_route_util::nearestDistanceToLineSegment ( double &  min_distance_from_line, double &  min_distance_on_line, const tf::Vector3 &  p0, const tf::Vector3 &  p1, const tf::Vector3 &  p, bool  extrapolate_start, bool  extrapolate_end  )

static

Definition at line 183 of file util.cpp.

nearestDistanceToLineSegment() [2/2]

static void swri_route_util::nearestDistanceToLineSegment ( double &  min_distance_from_line, double &  min_distance_on_line, const tf::Vector3 &  p0, const tf::Vector3 &  p1, const tf::Vector3 &  p, bool  extrapolate_start, bool  extrapolate_end  )

static

Definition at line 244 of file plan_util.cpp.

normalizePathPosition()

static void swri_route_util::normalizePathPosition ( const marti_nav_msgs::Path &  path, PathPosition &  position  )

static

Definition at line 145 of file path_util.cpp.

normalizePlanPosition()

void swri_route_util::normalizePlanPosition	(	marti_nav_msgs::PlanPosition &	position,
		const marti_nav_msgs::Plan &	path
	)

Definition at line 154 of file plan_util.cpp.

normalizeRoutePosition() [1/2]

bool swri_route_util::normalizeRoutePosition	(	marti_nav_msgs::RoutePosition &	normalized_position,
		const Route &	route,
		const marti_nav_msgs::RoutePosition &	position
	)

normalizeRoutePosition() [2/2]

bool swri_route_util::normalizeRoutePosition	(	mnm::RoutePosition &	normalized_position,
		const Route &	route,
		const mnm::RoutePosition &	position
	)

Definition at line 492 of file util.cpp.

planDistance() [1/2]

bool swri_route_util::planDistance	(	double &	distance,
		const marti_nav_msgs::PlanPosition &	start,
		const marti_nav_msgs::PlanPosition &	end,
		const marti_nav_msgs::Plan &	route
	)

planDistance() [2/2]

bool swri_route_util::planDistance	(	double &	distance,
		const marti_nav_msgs::PlanPosition &	start,
		const marti_nav_msgs::PlanPosition &	end,
		const mnm::Plan &	route
	)

Definition at line 504 of file plan_util.cpp.

pointFromMsg()

static void swri_route_util::pointFromMsg ( RoutePoint &  dst, const marti_nav_msgs::RoutePoint &  src  )

static

Definition at line 43 of file route.cpp.

projectOntoPlan() [1/2]

bool swri_route_util::projectOntoPlan	(	marti_nav_msgs::PlanPosition &	position,
		const marti_nav_msgs::Plan &	route,
		const tf::Vector3 &	point,
		bool	extrapolate_before_start,
		bool	extrapolate_past_end
	)

projectOntoPlan() [2/2]

bool swri_route_util::projectOntoPlan	(	mnm::PlanPosition &	position,
		const mnm::Plan &	route,
		const tf::Vector3 &	point,
		bool	extrapolate_before_start,
		bool	extrapolate_past_end
	)

Definition at line 281 of file plan_util.cpp.

projectOntoPlanWindow()

bool swri_route_util::projectOntoPlanWindow	(	marti_nav_msgs::PlanPosition &	position,
		const marti_nav_msgs::Plan &	route,
		const tf::Vector3 &	point,
		const marti_nav_msgs::PlanPosition &	window_start,
		const marti_nav_msgs::PlanPosition &	window_end
	)

Definition at line 366 of file plan_util.cpp.

projectOntoRoute() [1/2]

bool swri_route_util::projectOntoRoute	(	marti_nav_msgs::RoutePosition &	position,
		const Route &	route,
		const tf::Vector3 &	point,
		bool	extrapolate_before_start,
		bool	extrapolate_past_end
	)

projectOntoRoute() [2/2]

bool swri_route_util::projectOntoRoute	(	mnm::RoutePosition &	position,
		const Route &	route,
		const tf::Vector3 &	point,
		bool	extrapolate_before_start,
		bool	extrapolate_past_end
	)

Definition at line 220 of file util.cpp.

projectOntoRouteWindow() [1/2]

bool swri_route_util::projectOntoRouteWindow	(	marti_nav_msgs::RoutePosition &	position,
		const Route &	route,
		const tf::Vector3 &	point,
		const marti_nav_msgs::RoutePosition &	window_start,
		const marti_nav_msgs::RoutePosition &	window_end
	)

projectOntoRouteWindow() [2/2]

bool swri_route_util::projectOntoRouteWindow	(	mnm::RoutePosition &	position,
		const Route &	route,
		const tf::Vector3 &	point,
		const mnm::RoutePosition &	window_start,
		const mnm::RoutePosition &	window_end
	)

Definition at line 307 of file util.cpp.

projectToXY() [1/2]

void swri_route_util::projectToXY

(

marti_nav_msgs::Plan &

route

)

Definition at line 59 of file plan_util.cpp.

projectToXY() [2/2]

void swri_route_util::projectToXY

(

Route &

route

)

Definition at line 52 of file util.cpp.

routeDistance() [1/2]

bool swri_route_util::routeDistance	(	double &	distance,
		const marti_nav_msgs::RoutePosition &	start,
		const marti_nav_msgs::RoutePosition &	end,
		const Route &	route
	)

routeDistance() [2/2]

bool swri_route_util::routeDistance	(	double &	distance,
		const mnm::RoutePosition &	start,
		const mnm::RoutePosition &	end,
		const Route &	route
	)

Definition at line 604 of file util.cpp.

routeDistances() [1/2]

bool swri_route_util::routeDistances	(	std::vector< double > &	distances,
		const marti_nav_msgs::RoutePosition &	start,
		const std::vector< marti_nav_msgs::RoutePosition > &	ends,
		const Route &	route
	)

routeDistances() [2/2]

bool swri_route_util::routeDistances	(	std::vector< double > &	distances,
		const mnm::RoutePosition &	start,
		const std::vector< mnm::RoutePosition > &	ends,
		const Route &	route
	)

Definition at line 642 of file util.cpp.

speedsForCurvature() [1/2]

void swri_route_util::speedsForCurvature	(	marti_nav_msgs::RouteSpeedArray &	speeds,
		const Route &	route,
		const SpeedForCurvatureParameters &	parameters
	)

speedsForCurvature() [2/2]

void swri_route_util::speedsForCurvature	(	mnm::RouteSpeedArray &	speeds,
		const Route &	route,
		const SpeedForCurvatureParameters &	parameters
	)

Definition at line 243 of file route_speeds.cpp.

speedsForObstacles() [1/2]

void swri_route_util::speedsForObstacles	(	marti_nav_msgs::RouteSpeedArray &	speeds,
		std::vector< DistanceReport > &	reports,
		const Route &	route,
		const marti_nav_msgs::RoutePosition &	route_position,
		const std::vector< ObstacleData > &	obstacles,
		const SpeedForObstaclesParameters &	parameters
	)

speedsForObstacles() [2/2]

void swri_route_util::speedsForObstacles	(	mnm::RouteSpeedArray &	speeds,
		std::vector< DistanceReport > &	reports,
		const Route &	route,
		const mnm::RoutePosition &	route_position,
		const std::vector< ObstacleData > &	obstacles,
		const SpeedForObstaclesParameters &	p
	)

Definition at line 365 of file route_speeds.cpp.

transform() [1/3]

void swri_route_util::transform	(	marti_nav_msgs::Path &	path,
		const swri_transform_util::Transform &	transform,
		const std::string &	target_frame
	)

Definition at line 42 of file path_util.cpp.

transform() [2/3]

void swri_route_util::transform	(	marti_nav_msgs::Plan &	path,
		const swri_transform_util::Transform &	transform,
		const std::string &	target_frame
	)

Definition at line 42 of file plan_util.cpp.

transform() [3/3]

void swri_route_util::transform	(	Route &	route,
		const swri_transform_util::Transform &	transform,
		const std::string &	target_frame
	)

Definition at line 41 of file util.cpp.