Classes | Typedefs | Functions
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< RouteRoutePtr
 

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::Plan &path, const double x, const double y, marti_nav_msgs::PlanPosition &nearest_position, double &nearest_separation)
 
bool findLocalNearestDistanceForward (const marti_nav_msgs::Path &path, const double x, const double y, PathPosition &nearest_position, double &nearest_separation)
 
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 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 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 &point, const Route &route, const marti_nav_msgs::RoutePosition &position, bool allow_extrapolation)
 
bool interpolateRoutePosition (RoutePoint &dst, const Route &route, const mnm::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 (Route &route)
 
void projectToXY (marti_nav_msgs::Plan &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

Definition at line 171 of file route.h.

◆ 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::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.

◆ findLocalNearestDistanceForward() [2/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.

◆ generateObstacleData() [1/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() [2/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 
)

◆ generateObstacleData() [3/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() [4/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.

◆ 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 point,
const Route route,
const marti_nav_msgs::RoutePosition &  position,
bool  allow_extrapolation 
)

◆ interpolateRoutePosition() [2/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.

◆ 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 ( Route route)

Definition at line 52 of file util.cpp.

◆ projectToXY() [2/2]

void swri_route_util::projectToXY ( marti_nav_msgs::Plan &  route)

Definition at line 59 of file plan_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.



swri_route_util
Author(s):
autogenerated on Sat Jan 21 2023 03:13:29