Path tracking controller for car like robots, based on fuzzy logic. More...

#include <fuzzy_ptc.h>

Public Types
enum	RearSteeringMode { none, counter, crab, hybrid }
	rear steering mode enum More...

Public Member Functions
bool	computeVelocityCommands (const std::vector< geometry_msgs::PoseStamped > &path, geometry_msgs::Twist &cmd)
	Computes velocity commands. More...

	FuzzyPTC (std::string name)

bool	isGoalReached (const std::vector< geometry_msgs::PoseStamped > &path)
	Checks if the goal position and orientation have been reached. More...

void	reconfigure (RSBandPlannerConfig &config)
	Reconfigures controller params. More...

	~FuzzyPTC ()
	Destructor. More...

Private Member Functions
double	calcAngularDeviationError (const std::vector< geometry_msgs::PoseStamped > &path, unsigned int subGoalIdx)
	Calculates angular deviation from subgoal. More...

double	calcDistance (const std::vector< geometry_msgs::PoseStamped > &path, unsigned int idx1, unsigned int idx2)
	Calculates the distance between two path waypoints. More...

double	calcLateralDeviationError (const std::vector< geometry_msgs::PoseStamped > &path, unsigned int subGoalIdx)
	Calculates the lateral deviation from the sub goal. More...

double	calcOrientationError (const std::vector< geometry_msgs::PoseStamped > &path, unsigned int subGoalIdx)
	Calculates the orientation error between the robot and the sub goal. More...

double	calcPositionError (const std::vector< geometry_msgs::PoseStamped > &path, unsigned int subGoalIdx)
	Calculates the distance between the robot and the sub goal. More...

unsigned int	findSubGoal (const std::vector< geometry_msgs::PoseStamped > &path)
	Finds the current sub goal to follow. More...

void	initialize ()
	Initializes controller. More...

void	initializeFuzzyEngine ()
	Initializes fuzzy engine. More...

bool	isCuspPoint (const std::vector< geometry_msgs::PoseStamped > &path, unsigned int idx)
	Checks whether a path waypoint is a cusp point. More...

Private Attributes
fl::InputVariable *	angularDeviationError_
	robot to goal direction orientation error input variable More...

fl::InputVariable *	direction_
	direction (forwards/backwards) input variable More...

bool	displayControllerIO_
	display current trajectory information More...

fl::Engine *	engine_
	position FLC engine More...

fl::OutputVariable *	frontSteeringAngle_
	steering angle output variable 1 More...

std::vector< std::string >	frontSteeringRules_
	front steering rules More...

double	goalDistThreshold_
	distance to goal threshold More...

bool	initialized_
	variable indicating whether the controller has been initialized More...

double	latDevTolerance_
	lateral deviation tolerance More...

fl::InputVariable *	lateralDeviationError_
	lateral deviation error More...

double	maxSpeed_
	max speed More...

double	maxSteeringAngle_
	the max steering angle of the virtual middle wheel More...

fl::InputVariable *	orientationError_
	robot to final goal orientation error input variable More...

ros::NodeHandle *	pnh_
	private ros node handle More...

fl::InputVariable *	positionError_
	robot to goal distance error input variable More...

fl::OutputVariable *	rearSteeringDeviationAngle_
	rear steering deviation angle output variable More...

std::vector< std::string >	rearSteeringDeviationRules_
	rear steering deviation rules More...

RearSteeringMode	rearSteeringMode_
	rear wheel steering mode (none/counter/crab/hybrid) More...

fl::RuleBlock *	ruleBlock_
	flc rule block More...

fl::OutputVariable *	speed_
	speed output variable More...

std::vector< std::string >	speedRules_
	speed rules More...

bool	stop_
	if true controller returns zero velocity commands More...

visualization_msgs::Marker	subGoal_
	sub goal visualization marker More...

ros::Publisher	subGoalPub_
	publisher of the current sub goal marker More...

double	updateSubGoalDistThreshold_
	subGoal step More...

double	wheelbase_
	the robot wheelbase More...

double	xyGoalTolerance_
	xy goal tolerance threshold More...

double	yawGoalTolerance_
	yaw goal tolerance More...

Detailed Description

Path tracking controller for car like robots, based on fuzzy logic.

Definition at line 62 of file fuzzy_ptc.h.

Member Enumeration Documentation

enum rsband_local_planner::FuzzyPTC::RearSteeringMode

rear steering mode enum

Enumerator
none
counter
crab
hybrid

Definition at line 99 of file fuzzy_ptc.h.

Constructor & Destructor Documentation

rsband_local_planner::FuzzyPTC::FuzzyPTC ( std::string name )

Definition at line 62 of file fuzzy_ptc.cpp.

rsband_local_planner::FuzzyPTC::~FuzzyPTC ( )

Destructor.

Definition at line 69 of file fuzzy_ptc.cpp.

Member Function Documentation

double rsband_local_planner::FuzzyPTC::calcAngularDeviationError	(	const std::vector< geometry_msgs::PoseStamped > &	path,
		unsigned int	subGoalIdx
	)

private

Calculates angular deviation from subgoal.

Parameters

path	The path that the controller tracks
subGoalIdx	The current sub goal to track

Returns: The angular deviation from the subgoal

Definition at line 415 of file fuzzy_ptc.cpp.

double rsband_local_planner::FuzzyPTC::calcDistance	(	const std::vector< geometry_msgs::PoseStamped > &	path,
		unsigned int	idx1,
		unsigned int	idx2
	)

private

Calculates the distance between two path waypoints.

Parameters

path	The path that the controller tracks
idx1	The path idx to the first waypoint
idx2	The path idx to the second waypoint

Returns: The distance between the two path waypoints

Definition at line 449 of file fuzzy_ptc.cpp.

double rsband_local_planner::FuzzyPTC::calcLateralDeviationError	(	const std::vector< geometry_msgs::PoseStamped > &	path,
		unsigned int	subGoalIdx
	)

private

Calculates the lateral deviation from the sub goal.

Parameters

path	The path that the controller tracks
subGoalIdx	The path idx to the sub goal

Returns: The lateral deviation from the sub goal

Definition at line 440 of file fuzzy_ptc.cpp.

double rsband_local_planner::FuzzyPTC::calcOrientationError	(	const std::vector< geometry_msgs::PoseStamped > &	path,
		unsigned int	subGoalIdx
	)

private

Calculates the orientation error between the robot and the sub goal.

Parameters

path	The path that the controller tracks
subGoalIdx	The path index to the current sub goal

Returns: The orientation error between the robot and the sub goal

Definition at line 424 of file fuzzy_ptc.cpp.

double rsband_local_planner::FuzzyPTC::calcPositionError	(	const std::vector< geometry_msgs::PoseStamped > &	path,
		unsigned int	subGoalIdx
	)

private

Calculates the distance between the robot and the sub goal.

Parameters

path	The path that the controller tracks
subGoalIdx	The path idx to the sub goal

Returns: The distance between the robot and the sub goal

Definition at line 432 of file fuzzy_ptc.cpp.

bool rsband_local_planner::FuzzyPTC::computeVelocityCommands	(	const std::vector< geometry_msgs::PoseStamped > &	path,
		geometry_msgs::Twist &	cmd
	)

Computes velocity commands.

Parameters

path	The path to track
cmd	The command that will be returned by the controller

Returns: true if a command was produced successfully

Definition at line 306 of file fuzzy_ptc.cpp.

unsigned int rsband_local_planner::FuzzyPTC::findSubGoal ( const std::vector< geometry_msgs::PoseStamped > & path )

private

Finds the current sub goal to follow.

Parameters

path	The path that the controller tracks

Returns: The path index to the current sub goal to follow

Definition at line 462 of file fuzzy_ptc.cpp.

void rsband_local_planner::FuzzyPTC::initialize ( )

private

Initializes controller.

Definition at line 77 of file fuzzy_ptc.cpp.

void rsband_local_planner::FuzzyPTC::initializeFuzzyEngine ( )

private

Initializes fuzzy engine.

Definition at line 151 of file fuzzy_ptc.cpp.

bool rsband_local_planner::FuzzyPTC::isCuspPoint	(	const std::vector< geometry_msgs::PoseStamped > &	path,
		unsigned int	idx
	)

private

Checks whether a path waypoint is a cusp point.

Parameters

path	The path that the controller tracks
idx	The point to check if it is a cusp point return true if given path waypoint is a cusp point

Definition at line 488 of file fuzzy_ptc.cpp.

bool rsband_local_planner::FuzzyPTC::isGoalReached ( const std::vector< geometry_msgs::PoseStamped > & path )

Checks if the goal position and orientation have been reached.

Parameters

path	The path to follow

Returns: true: if the goal has been reached

Definition at line 404 of file fuzzy_ptc.cpp.

void rsband_local_planner::FuzzyPTC::reconfigure ( RSBandPlannerConfig & config )

Reconfigures controller params.

Definition at line 131 of file fuzzy_ptc.cpp.

Member Data Documentation

fl::InputVariable* rsband_local_planner::FuzzyPTC::angularDeviationError_

private

robot to goal direction orientation error input variable

Definition at line 227 of file fuzzy_ptc.h.

fl::InputVariable* rsband_local_planner::FuzzyPTC::direction_

private

direction (forwards/backwards) input variable

Definition at line 225 of file fuzzy_ptc.h.

bool rsband_local_planner::FuzzyPTC::displayControllerIO_

private

display current trajectory information

Definition at line 215 of file fuzzy_ptc.h.

fl::Engine* rsband_local_planner::FuzzyPTC::engine_

private

position FLC engine

Definition at line 222 of file fuzzy_ptc.h.

fl::OutputVariable* rsband_local_planner::FuzzyPTC::frontSteeringAngle_

private

steering angle output variable 1

Definition at line 235 of file fuzzy_ptc.h.

std::vector<std::string> rsband_local_planner::FuzzyPTC::frontSteeringRules_

private

front steering rules

Definition at line 247 of file fuzzy_ptc.h.

double rsband_local_planner::FuzzyPTC::goalDistThreshold_

private

distance to goal threshold

Definition at line 209 of file fuzzy_ptc.h.

bool rsband_local_planner::FuzzyPTC::initialized_

private

variable indicating whether the controller has been initialized

Definition at line 188 of file fuzzy_ptc.h.

double rsband_local_planner::FuzzyPTC::latDevTolerance_

private

lateral deviation tolerance

Definition at line 211 of file fuzzy_ptc.h.

fl::InputVariable* rsband_local_planner::FuzzyPTC::lateralDeviationError_

private

lateral deviation error

Definition at line 233 of file fuzzy_ptc.h.

double rsband_local_planner::FuzzyPTC::maxSpeed_

private

max speed

Definition at line 203 of file fuzzy_ptc.h.

double rsband_local_planner::FuzzyPTC::maxSteeringAngle_

private

the max steering angle of the virtual middle wheel

Definition at line 201 of file fuzzy_ptc.h.

fl::InputVariable* rsband_local_planner::FuzzyPTC::orientationError_

private

robot to final goal orientation error input variable

Definition at line 229 of file fuzzy_ptc.h.

ros::NodeHandle* rsband_local_planner::FuzzyPTC::pnh_

private

private ros node handle

Definition at line 185 of file fuzzy_ptc.h.

fl::InputVariable* rsband_local_planner::FuzzyPTC::positionError_

private

robot to goal distance error input variable

Definition at line 231 of file fuzzy_ptc.h.

fl::OutputVariable* rsband_local_planner::FuzzyPTC::rearSteeringDeviationAngle_

private

rear steering deviation angle output variable

Definition at line 237 of file fuzzy_ptc.h.

std::vector<std::string> rsband_local_planner::FuzzyPTC::rearSteeringDeviationRules_

private

rear steering deviation rules

Definition at line 249 of file fuzzy_ptc.h.

RearSteeringMode rsband_local_planner::FuzzyPTC::rearSteeringMode_

private

rear wheel steering mode (none/counter/crab/hybrid)

Definition at line 196 of file fuzzy_ptc.h.

fl::RuleBlock* rsband_local_planner::FuzzyPTC::ruleBlock_

private

flc rule block

Definition at line 242 of file fuzzy_ptc.h.

fl::OutputVariable* rsband_local_planner::FuzzyPTC::speed_

private

speed output variable

Definition at line 239 of file fuzzy_ptc.h.

std::vector<std::string> rsband_local_planner::FuzzyPTC::speedRules_

private

speed rules

Definition at line 245 of file fuzzy_ptc.h.

bool rsband_local_planner::FuzzyPTC::stop_

private

if true controller returns zero velocity commands

Definition at line 217 of file fuzzy_ptc.h.

visualization_msgs::Marker rsband_local_planner::FuzzyPTC::subGoal_

private

sub goal visualization marker

Definition at line 193 of file fuzzy_ptc.h.

ros::Publisher rsband_local_planner::FuzzyPTC::subGoalPub_

private

publisher of the current sub goal marker

Definition at line 191 of file fuzzy_ptc.h.

double rsband_local_planner::FuzzyPTC::updateSubGoalDistThreshold_

private

subGoal step

Definition at line 213 of file fuzzy_ptc.h.

double rsband_local_planner::FuzzyPTC::wheelbase_

private

the robot wheelbase

Definition at line 199 of file fuzzy_ptc.h.

double rsband_local_planner::FuzzyPTC::xyGoalTolerance_

private

xy goal tolerance threshold

Definition at line 205 of file fuzzy_ptc.h.

double rsband_local_planner::FuzzyPTC::yawGoalTolerance_

private

yaw goal tolerance

Definition at line 207 of file fuzzy_ptc.h.

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

Public Types

Public Member Functions

Private Member Functions

Private Attributes

Detailed Description

Member Enumeration Documentation

Constructor & Destructor Documentation

Member Function Documentation

Member Data Documentation