mppi::critics::CostCritic Class Reference

Critic objective function for avoiding obstacles using costmap's inflated cost. More...

#include <nav2_mppi_controller/include/nav2_mppi_controller/critics/cost_critic.hpp>

Inheritance diagram for mppi::critics::CostCritic:

Collaboration diagram for mppi::critics::CostCritic:

Public Member Functions
void	initialize () override
	Initialize critic.
void	score (CriticData &data) override
	Evaluate cost related to obstacle avoidance. More...
Public Member Functions inherited from mppi::critics::CriticFunction
	CriticFunction ()=default
	Constructor for mppi::critics::CriticFunction.
virtual	~CriticFunction ()=default
	Destructor for mppi::critics::CriticFunction.
void	on_configure (rclcpp_lifecycle::LifecycleNode::WeakPtr parent, const std::string &parent_name, const std::string &name, std::shared_ptr< nav2_costmap_2d::Costmap2DROS > costmap_ros, ParametersHandler *param_handler)
	Configure critic on bringup. More...
std::string	getName ()
	Get name of critic.

Protected Member Functions
bool	inCollision (float cost, float x, float y, float theta)
	Checks if cost represents a collision. More...
float	findCircumscribedCost (std::shared_ptr< nav2_costmap_2d::Costmap2DROS > costmap)
	Find the min cost of the inflation decay function for which the robot MAY be in collision in any orientation. More...
bool	worldToMapFloat (float wx, float wy, unsigned int &mx, unsigned int &my) const
	An implementation of worldToMap fully using floats. More...
unsigned int	getIndex (unsigned int mx, unsigned int my) const
	A local implementation of getIndex. More...

Protected Attributes
nav2_costmap_2d::FootprintCollisionChecker< nav2_costmap_2d::Costmap2D * >	collision_checker_ {nullptr}
float	possible_collision_cost_
bool	consider_footprint_ {true}
bool	is_tracking_unknown_ {true}
float	circumscribed_radius_ {0.0f}
float	circumscribed_cost_ {0.0f}
float	collision_cost_ {0.0f}
float	critical_cost_ {0.0f}
unsigned int	near_collision_cost_ {253}
float	weight_ {0}
unsigned int	trajectory_point_step_
float	origin_x_
float	origin_y_
float	resolution_
unsigned int	size_x_
unsigned int	size_y_
float	near_goal_distance_
std::string	inflation_layer_name_
unsigned int	power_ {0}
bool	enforce_path_inversion_ {false}
Protected Attributes inherited from mppi::critics::CriticFunction
bool	enabled_
std::string	name_
std::string	parent_name_
rclcpp_lifecycle::LifecycleNode::WeakPtr	parent_
std::shared_ptr< nav2_costmap_2d::Costmap2DROS >	costmap_ros_
nav2_costmap_2d::Costmap2D *	costmap_ {nullptr}
ParametersHandler *	parameters_handler_
rclcpp::Logger	logger_ {rclcpp::get_logger("MPPIController")}

Detailed Description

Critic objective function for avoiding obstacles using costmap's inflated cost.

Definition at line 35 of file cost_critic.hpp.

Member Function Documentation

findCircumscribedCost()

float mppi::critics::CostCritic::findCircumscribedCost ( std::shared_ptr< nav2_costmap_2d::Costmap2DROS >  costmap )

inlineprotected

Find the min cost of the inflation decay function for which the robot MAY be in collision in any orientation.

Parameters

costmap

Costmap2DROS to get minimum inscribed cost (e.g. 128 in inflation layer documentation)

Returns

double circumscribed cost, any higher than this and need to do full footprint collision checking since some element of the robot could be in collision

Definition at line 86 of file cost_critic.cpp.

Referenced by initialize(), and score().

Here is the caller graph for this function:

getIndex()

unsigned int mppi::critics::CostCritic::getIndex ( unsigned int  mx, unsigned int  my  ) const

inlineprotected

A local implementation of getIndex.

Parameters

mx	unsigned int map X coord
my	unsigned into map Y coord

Returns

Index

Definition at line 121 of file cost_critic.hpp.

Referenced by nav2_simple_commander.costmap_2d.PyCostmap2D::getCostXY(), score(), and nav2_simple_commander.costmap_2d.PyCostmap2D::setCost().

Here is the caller graph for this function:

inCollision()

bool mppi::critics::CostCritic::inCollision ( float  cost, float  x, float  y, float  theta  )

inlineprotected

Checks if cost represents a collision.

Parameters

cost	Point cost at pose center
x	X of pose
y	Y of pose
theta	theta of pose

Returns

bool if in collision

Definition at line 59 of file cost_critic.hpp.

References nav2_costmap_2d::FootprintCollisionChecker< CostmapT >::footprintCostAtPose().

Referenced by score().

Here is the call graph for this function:

Here is the caller graph for this function:

score()

void mppi::critics::CostCritic::score ( CriticData &  data )

overridevirtual

Evaluate cost related to obstacle avoidance.

Parameters

costs

[out] add obstacle cost values to this tensor

Implements mppi::critics::CriticFunction.

Definition at line 132 of file cost_critic.cpp.

References findCircumscribedCost(), nav2_costmap_2d::FootprintCollisionChecker< CostmapT >::getCostmap(), getIndex(), inCollision(), and worldToMapFloat().

Here is the call graph for this function:

worldToMapFloat()

bool mppi::critics::CostCritic::worldToMapFloat ( float  wx, float  wy, unsigned int &  mx, unsigned int &  my  ) const

inlineprotected

An implementation of worldToMap fully using floats.

Parameters

wx	Float world X coord
wy	Float world Y coord
mx	unsigned int map X coord
my	unsigned into map Y coord

Returns

if successful

Definition at line 100 of file cost_critic.hpp.

Referenced by score().

Here is the caller graph for this function:

---

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

• nav2_mppi_controller/include/nav2_mppi_controller/critics/cost_critic.hpp
• nav2_mppi_controller/src/critics/cost_critic.cpp