dubins_local.cpp File Reference

Contains Function definition of dubins local planner. More...

#include <iostream>
#include <cstdlib>
#include <vector>
#include <cmath>
#include "dubins_local.h"
#include "utils.hpp"

Include dependency graph for dubins_local.cpp:

Functions
double	sinc (double t)
double	mod2pi (double ang)
double	rangeSymm (double const &ang)
bool	check (double const &s1, double const &k0, double const &s2, double const &k1, double const &s3, double const &k2, double const &th0, double const &thf)
void	scaleToStandard (double x0, double y0, double th0, double xf, double yf, double thf, double kmax, double &sc_th0, double &sc_thf, double &sc_kmax, double &lambda)
void	scaleFromStandard (double lambda, double sc_s1, double sc_s2, double sc_s3, double &s1, double &s2, double &s3)
void	LSL (double sc_th0, double sc_thf, double sc_kmax, bool &ok, double &sc_s1, double &sc_s2, double &sc_s3)
	Compute the arcs for LSL configuration. More...
void	RSR (double sc_th0, double sc_thf, double sc_kmax, bool &ok, double &sc_s1, double &sc_s2, double &sc_s3)
	Compute the arcs for RSR configuration. More...
void	LSR (double sc_th0, double sc_thf, double sc_kmax, bool &ok, double &sc_s1, double &sc_s2, double &sc_s3)
	Compute the arcs for LSR configuration. More...
void	RSL (double sc_th0, double sc_thf, double sc_kmax, bool &ok, double &sc_s1, double &sc_s2, double &sc_s3)
	Compute the arcs for RSL configuration. More...
void	RLR (double sc_th0, double sc_thf, double sc_kmax, bool &ok, double &sc_s1, double &sc_s2, double &sc_s3)
	Compute the arcs for RLR configuration. More...
void	LRL (double sc_th0, double sc_thf, double sc_kmax, bool &ok, double &sc_s1, double &sc_s2, double &sc_s3)
	Compute the arcs for LRL configuration. More...
void	set_dubinsArc (dubinsArc &ptr, double x0, double y0, double th0, double k, double s)
void	set_dubinsCurve (dubinsCurve &curve_ptr, double x0, double y0, double th0, double s1, double s2, double s3, double k0, double k1, double k2)
void	dubins_shortest_path (dubinsCurve &curve, double const &x0, double const &y0, double const &th0, double const &xf, double const &yf, double const &thf, double const &kmax)
	Compute shortest path using dunins confiuration. More...
Path	getPath (dubinsCurve c, int npts)
	Discretize the dubins curve to points. More...

Detailed Description

Contains Function definition of dubins local planner.

Author

Aravind Swaminathan

Date

10-Jan-2020

Function Documentation

bool check	(	double const &	s1,
		double const &	k0,
		double const &	s2,
		double const &	k1,
		double const &	s3,
		double const &	k2,
		double const &	th0,
		double const &	thf
	)

void dubins_shortest_path	(	dubinsCurve &	curve,
		double const &	x0,
		double const &	y0,
		double const &	th0,
		double const &	xf,
		double const &	yf,
		double const &	thf,
		double const &	kmax
	)

Compute shortest path using dunins confiuration.

Parameters

curve	Output curve
x0	Start location x
y0	Start location y
th0	Start location theta
xf	End location x
yf	End location y
thf	End location theta
kmax	Maximum curvature of the robot

Path getPath	(	dubinsCurve	curve,
		int	npts
	)

Discretize the dubins curve to points.

Parameters

curve	The dubins curve obtained
npts	Number of points for the whole curve

Returns

Output path structure

void LRL	(	double	sc_th0,
		double	sc_thf,
		double	sc_kmax,
		bool &	ok,
		double &	sc_s1,
		double &	sc_s2,
		double &	sc_s3
	)

Compute the arcs for LRL configuration.

Parameters

sc_th0	Start theta
sc_thf	Final theta
sc_kmax	Curvature max
ok	Check if this configuration fits the given input
sc_s1	Arc 1 coeff
sc_s2	Arc 2 coeff
sc_s3	Arc 3 coeff

void LSL	(	double	sc_th0,
		double	sc_thf,
		double	sc_kmax,
		bool &	ok,
		double &	sc_s1,
		double &	sc_s2,
		double &	sc_s3
	)

Compute the arcs for LSL configuration.

Parameters

sc_th0	Start theta
sc_thf	Final theta
sc_kmax	Curvature max
ok	Check if this configuration fits the given input
sc_s1	Arc 1 coeff
sc_s2	Arc 2 coeff
sc_s3	Arc 3 coeff

void LSR	(	double	sc_th0,
		double	sc_thf,
		double	sc_kmax,
		bool &	ok,
		double &	sc_s1,
		double &	sc_s2,
		double &	sc_s3
	)

Compute the arcs for LSR configuration.

Parameters

sc_th0	Start theta
sc_thf	Final theta
sc_kmax	Curvature max
ok	Check if this configuration fits the given input
sc_s1	Arc 1 coeff
sc_s2	Arc 2 coeff
sc_s3	Arc 3 coeff

double mod2pi

(

double

ang

)

double rangeSymm

(

double const &

ang

)

void RLR	(	double	sc_th0,
		double	sc_thf,
		double	sc_kmax,
		bool &	ok,
		double &	sc_s1,
		double &	sc_s2,
		double &	sc_s3
	)

Compute the arcs for RLR configuration.

Parameters

sc_th0	Start theta
sc_thf	Final theta
sc_kmax	Curvature max
ok	Check if this configuration fits the given input
sc_s1	Arc 1 coeff
sc_s2	Arc 2 coeff
sc_s3	Arc 3 coeff

void RSL	(	double	sc_th0,
		double	sc_thf,
		double	sc_kmax,
		bool &	ok,
		double &	sc_s1,
		double &	sc_s2,
		double &	sc_s3
	)

Compute the arcs for RSL configuration.

Parameters

sc_th0	Start theta
sc_thf	Final theta
sc_kmax	Curvature max
ok	Check if this configuration fits the given input
sc_s1	Arc 1 coeff
sc_s2	Arc 2 coeff
sc_s3	Arc 3 coeff

void RSR	(	double	sc_th0,
		double	sc_thf,
		double	sc_kmax,
		bool &	ok,
		double &	sc_s1,
		double &	sc_s2,
		double &	sc_s3
	)

Compute the arcs for RSR configuration.

Parameters

sc_th0	Start theta
sc_thf	Final theta
sc_kmax	Curvature max
ok	Check if this configuration fits the given input
sc_s1	Arc 1 coeff
sc_s2	Arc 2 coeff
sc_s3	Arc 3 coeff

void scaleFromStandard	(	double	lambda,
		double	sc_s1,
		double	sc_s2,
		double	sc_s3,
		double &	s1,
		double &	s2,
		double &	s3
	)

void scaleToStandard	(	double	x0,
		double	y0,
		double	th0,
		double	xf,
		double	yf,
		double	thf,
		double	kmax,
		double &	sc_th0,
		double &	sc_thf,
		double &	sc_kmax,
		double &	lambda
	)

void set_dubinsArc	(	dubinsArc &	ptr,
		double	x0,
		double	y0,
		double	th0,
		double	k,
		double	s
	)

void set_dubinsCurve	(	dubinsCurve &	curve_ptr,
		double	x0,
		double	y0,
		double	th0,
		double	s1,
		double	s2,
		double	s3,
		double	k0,
		double	k1,
		double	k2
	)

double sinc

(

double

t

)