Implements both nav_core::BaseLocalPlanner and mbf_costmap_core::CostmapController abstract interfaces, so the teb_local_planner plugin can be used both in move_base and move_base_flex (MBF). More...

#include <mpc_local_planner_ros.h>

Inheritance diagram for mpc_local_planner::MpcLocalPlannerROS:

[legend]

Classes
struct	CostmapConverterPlugin

struct	Parameters

Public Member Functions
bool	cancel ()
	Requests the planner to cancel, e.g. if it takes too much time. More...

uint32_t	computeVelocityCommands (const geometry_msgs::PoseStamped &pose, const geometry_msgs::TwistStamped &velocity, geometry_msgs::TwistStamped &cmd_vel, std::string &message)
	Given the current position, orientation, and velocity of the robot, compute velocity commands to send to the base. More...

bool	computeVelocityCommands (geometry_msgs::Twist &cmd_vel)
	Given the current position, orientation, and velocity of the robot, compute velocity commands to send to the base. More...

void	initialize (std::string name, tf2_ros::Buffer tf, costmap_2d::Costmap2DROS costmap_ros)
	Initializes the teb plugin. More...

bool	isGoalReached ()
	Check if the goal pose has been achieved. More...

bool	isGoalReached (double xy_tolerance, double yaw_tolerance)
	Dummy version to satisfy MBF API. More...

	MpcLocalPlannerROS ()
	Default constructor of the plugin. More...

bool	setPlan (const std::vector< geometry_msgs::PoseStamped > &orig_global_plan)
	Set the plan that the teb local planner is following. More...

	~MpcLocalPlannerROS ()
	Destructor of the plugin. More...

Public Member Functions inherited from nav_core::BaseLocalPlanner
virtual	~BaseLocalPlanner ()

Public Member Functions inherited from mbf_costmap_core::CostmapController
virtual void	initialize (std::string name, ::TF tf, costmap_2d::Costmap2DROS costmap_ros)=0

virtual	~CostmapController ()

Public Member Functions inherited from mbf_abstract_core::AbstractController
virtual	~AbstractController ()

Private Types
using	CircularObstacle = teb_local_planner::CircularObstacle

using	LineObstacle = teb_local_planner::LineObstacle

using	ObstaclePtr = teb_local_planner::ObstaclePtr

using	ObstContainer = teb_local_planner::ObstContainer

using	Point2dContainer = teb_local_planner::Point2dContainer

using	PointObstacle = teb_local_planner::PointObstacle

using	PolygonObstacle = teb_local_planner::PolygonObstacle

using	PoseSE2 = teb_local_planner::PoseSE2

using	RobotFootprintModelPtr = teb_local_planner::RobotFootprintModelPtr

using	ViaPointContainer = std::vector< PoseSE2 >

Public utility functions/methods
costmap_2d::Costmap2DROS *	_costmap_ros
	Pointer to the costmap ros wrapper, received from the navigation stack. More...

costmap_2d::Costmap2D *	_costmap
	Pointer to the 2d costmap (obtained from the costmap ros wrapper) More...

tf2_ros::Buffer *	_tf
	pointer to tf buffer More...

Controller	_controller

ObstContainer	_obstacles
	Obstacle vector that should be considered during local trajectory optimization. More...

Publisher	_publisher

std::shared_ptr< base_local_planner::CostmapModel >	_costmap_model

corbo::TimeSeries::Ptr	_x_seq = std::make_shared<corbo::TimeSeries>()

corbo::TimeSeries::Ptr	_u_seq = std::make_shared<corbo::TimeSeries>()

std::vector< geometry_msgs::PoseStamped >	_global_plan
	Store the current global plan. More...

base_local_planner::OdometryHelperRos	_odom_helper
	Provides an interface to receive the current velocity from the robot. More...

pluginlib::ClassLoader< costmap_converter::BaseCostmapToPolygons >	_costmap_converter_loader
	Load costmap converter plugins at runtime. More...

boost::shared_ptr< costmap_converter::BaseCostmapToPolygons >	_costmap_converter
	Store the current costmap_converter. More...

ros::Subscriber	_custom_obst_sub
	Subscriber for custom obstacles received via a ObstacleMsg. More...

std::mutex	_custom_obst_mutex
	Mutex that locks the obstacle array (multi-threaded) More...

costmap_converter::ObstacleArrayMsg	_custom_obstacle_msg
	Copy of the most recent obstacle message. More...

ViaPointContainer	_via_points

ros::Subscriber	_via_points_sub
	Subscriber for custom via-points received via a Path msg. More...

bool	_custom_via_points_active = false
	Keep track whether valid via-points have been received from via_points_sub_. More...

std::mutex	_via_point_mutex
	Mutex that locks the via_points container (multi-threaded) More...

PoseSE2	_robot_pose
	Store current robot pose. More...

PoseSE2	_robot_goal
	Store current robot goal. More...

geometry_msgs::Twist	_robot_vel
	Store current robot translational and angular velocity (vx, vy, omega) More...

bool	_goal_reached = false
	store whether the goal is reached or not More...

ros::Time	_time_last_infeasible_plan
	Store at which time stamp the last infeasible plan was detected. More...

int	_no_infeasible_plans = 0
	Store how many times in a row the planner failed to find a feasible plan. More...

geometry_msgs::Twist	_last_cmd
	Store the last control command generated in computeVelocityCommands() More...

ros::Time	_time_last_cmd

RobotFootprintModelPtr	_robot_model

std::vector< geometry_msgs::Point >	_footprint_spec
	Store the footprint of the robot. More...

double	_robot_inscribed_radius
	The radius of the inscribed circle of the robot (collision possible) More...

double	_robot_circumscribed_radius
	The radius of the circumscribed circle of the robot. More...

std::string	_global_frame
	The frame in which the controller will run. More...

std::string	_robot_base_frame
	Used as the base frame id of the robot. More...

bool	_initialized
	Keeps track about the correct initialization of this class. More...

struct mpc_local_planner::MpcLocalPlannerROS::Parameters	_params

struct mpc_local_planner::MpcLocalPlannerROS::CostmapConverterPlugin	_costmap_conv_params

static Eigen::Vector2d	tfPoseToEigenVector2dTransRot (const tf::Pose &tf_vel)
	Transform a tf::Pose type into a Eigen::Vector2d containing the translational and angular velocities. More...

static RobotFootprintModelPtr	getRobotFootprintFromParamServer (const ros::NodeHandle &nh, costmap_2d::Costmap2DROS *costmap_ros=nullptr)
	Get the current robot footprint/contour model. More...

static RobotFootprintModelPtr	getRobotFootprintFromCostmap2d (costmap_2d::Costmap2DROS &costmap_ros)
	Get the current robot footprint/contour model. More...

static Point2dContainer	makeFootprintFromXMLRPC (XmlRpc::XmlRpcValue &footprint_xmlrpc, const std::string &full_param_name)
	Set the footprint from the given XmlRpcValue. More...

static double	getNumberFromXMLRPC (XmlRpc::XmlRpcValue &value, const std::string &full_param_name)
	Get a number from the given XmlRpcValue. More...

void	updateObstacleContainerWithCostmap ()
	Update internal obstacle vector based on occupied costmap cells. More...

void	updateObstacleContainerWithCostmapConverter ()
	Update internal obstacle vector based on polygons provided by a costmap_converter plugin. More...

void	updateObstacleContainerWithCustomObstacles ()
	Update internal obstacle vector based on custom messages received via subscriber. More...

void	updateViaPointsContainer (const std::vector< geometry_msgs::PoseStamped > &transformed_plan, double min_separation)
	Update internal via-point container based on the current reference plan. More...

void	customObstacleCB (const costmap_converter::ObstacleArrayMsg::ConstPtr &obst_msg)
	Callback for the dynamic_reconfigure node. More...

void	customViaPointsCB (const nav_msgs::Path::ConstPtr &via_points_msg)
	Callback for custom via-points. More...

bool	pruneGlobalPlan (const tf2_ros::Buffer &tf, const geometry_msgs::PoseStamped &global_pose, std::vector< geometry_msgs::PoseStamped > &global_plan, double dist_behind_robot=1)
	Prune global plan such that already passed poses are cut off. More...

bool	transformGlobalPlan (const tf2_ros::Buffer &tf, const std::vector< geometry_msgs::PoseStamped > &global_plan, const geometry_msgs::PoseStamped &global_pose, const costmap_2d::Costmap2D &costmap, const std::string &global_frame, double max_plan_length, std::vector< geometry_msgs::PoseStamped > &transformed_plan, int current_goal_idx=NULL, geometry_msgs::TransformStamped tf_plan_to_global=NULL) const
	Transforms the global plan of the robot from the planner frame to the local frame (modified). More...

double	estimateLocalGoalOrientation (const std::vector< geometry_msgs::PoseStamped > &global_plan, const geometry_msgs::PoseStamped &local_goal, int current_goal_idx, const geometry_msgs::TransformStamped &tf_plan_to_global, int moving_average_length=3) const
	Estimate the orientation of a pose from the global_plan that is treated as a local goal for the local planner. More...

void	validateFootprints (double opt_inscribed_radius, double costmap_inscribed_radius, double min_obst_dist)
	Validate current parameter values of the footprint for optimization, obstacle distance and the costmap footprint. More...

Additional Inherited Members
Public Types inherited from mbf_costmap_core::CostmapController
typedef boost::shared_ptr< ::mbf_costmap_core::CostmapController >	Ptr

Public Types inherited from mbf_abstract_core::AbstractController
typedef boost::shared_ptr< ::mbf_abstract_core::AbstractController >	Ptr

Protected Member Functions inherited from nav_core::BaseLocalPlanner
	BaseLocalPlanner ()

Protected Member Functions inherited from mbf_costmap_core::CostmapController
	CostmapController ()

Protected Member Functions inherited from mbf_abstract_core::AbstractController
	AbstractController ()

Detailed Description

Implements both nav_core::BaseLocalPlanner and mbf_costmap_core::CostmapController abstract interfaces, so the teb_local_planner plugin can be used both in move_base and move_base_flex (MBF).

Todo:: Escape behavior, more efficient obstacle handling

Definition at line 97 of file mpc_local_planner_ros.h.

Member Typedef Documentation

◆ CircularObstacle

using mpc_local_planner::MpcLocalPlannerROS::CircularObstacle = teb_local_planner::CircularObstacle

private

Definition at line 107 of file mpc_local_planner_ros.h.

◆ LineObstacle

using mpc_local_planner::MpcLocalPlannerROS::LineObstacle = teb_local_planner::LineObstacle

private

Definition at line 108 of file mpc_local_planner_ros.h.

◆ ObstaclePtr

using mpc_local_planner::MpcLocalPlannerROS::ObstaclePtr = teb_local_planner::ObstaclePtr

private

Definition at line 105 of file mpc_local_planner_ros.h.

◆ ObstContainer

using mpc_local_planner::MpcLocalPlannerROS::ObstContainer = teb_local_planner::ObstContainer

private

Definition at line 102 of file mpc_local_planner_ros.h.

◆ Point2dContainer

using mpc_local_planner::MpcLocalPlannerROS::Point2dContainer = teb_local_planner::Point2dContainer

private

Definition at line 101 of file mpc_local_planner_ros.h.

◆ PointObstacle

using mpc_local_planner::MpcLocalPlannerROS::PointObstacle = teb_local_planner::PointObstacle

private

Definition at line 106 of file mpc_local_planner_ros.h.

◆ PolygonObstacle

using mpc_local_planner::MpcLocalPlannerROS::PolygonObstacle = teb_local_planner::PolygonObstacle

private

Definition at line 109 of file mpc_local_planner_ros.h.

◆ PoseSE2

using mpc_local_planner::MpcLocalPlannerROS::PoseSE2 = teb_local_planner::PoseSE2

private

Definition at line 99 of file mpc_local_planner_ros.h.

◆ RobotFootprintModelPtr

using mpc_local_planner::MpcLocalPlannerROS::RobotFootprintModelPtr = teb_local_planner::RobotFootprintModelPtr

private

Definition at line 100 of file mpc_local_planner_ros.h.

◆ ViaPointContainer

using mpc_local_planner::MpcLocalPlannerROS::ViaPointContainer = std::vector<PoseSE2>

private

Definition at line 103 of file mpc_local_planner_ros.h.

Constructor & Destructor Documentation

◆ MpcLocalPlannerROS()

mpc_local_planner::MpcLocalPlannerROS::MpcLocalPlannerROS ( )

Default constructor of the plugin.

Definition at line 64 of file mpc_local_planner_ros.cpp.

◆ ~MpcLocalPlannerROS()

mpc_local_planner::MpcLocalPlannerROS::~MpcLocalPlannerROS ( )

Destructor of the plugin.

Definition at line 77 of file mpc_local_planner_ros.cpp.

Member Function Documentation

◆ cancel()

bool mpc_local_planner::MpcLocalPlannerROS::cancel ( )

inlinevirtual

Requests the planner to cancel, e.g. if it takes too much time.

Remarks: New on MBF API

Returns: True if a cancel has been successfully requested, false if not implemented.

Implements mbf_costmap_core::CostmapController.

Definition at line 192 of file mpc_local_planner_ros.h.

◆ computeVelocityCommands() [1/2]

uint32_t mpc_local_planner::MpcLocalPlannerROS::computeVelocityCommands	(	const geometry_msgs::PoseStamped &	pose,
		const geometry_msgs::TwistStamped &	velocity,
		geometry_msgs::TwistStamped &	cmd_vel,
		std::string &	message
	)

virtual

Given the current position, orientation, and velocity of the robot, compute velocity commands to send to the base.

Remarks: Extended version for MBF API

Parameters

pose	the current pose of the robot.
velocity	the current velocity of the robot.
cmd_vel	Will be filled with the velocity command to be passed to the robot base.
message	Optional more detailed outcome as a string

Returns: Result code as described on ExePath action result: SUCCESS = 0 1..9 are reserved as plugin specific non-error results FAILURE = 100 Unspecified failure, only used for old, non-mfb_core based plugins CANCELED = 101 NO_VALID_CMD = 102 PAT_EXCEEDED = 103 COLLISION = 104 OSCILLATION = 105 ROBOT_STUCK = 106 MISSED_GOAL = 107 MISSED_PATH = 108 BLOCKED_PATH = 109 INVALID_PATH = 110 TF_ERROR = 111 NOT_INITIALIZED = 112 INVALID_PLUGIN = 113 INTERNAL_ERROR = 114 121..149 are reserved as plugin specific errors

Implements mbf_costmap_core::CostmapController.

Definition at line 247 of file mpc_local_planner_ros.cpp.

◆ computeVelocityCommands() [2/2]

bool mpc_local_planner::MpcLocalPlannerROS::computeVelocityCommands ( geometry_msgs::Twist & cmd_vel )

virtual

Given the current position, orientation, and velocity of the robot, compute velocity commands to send to the base.

Parameters

cmd_vel Will be filled with the velocity command to be passed to the robot base

Returns: True if a valid trajectory was found, false otherwise

Implements nav_core::BaseLocalPlanner.

Definition at line 237 of file mpc_local_planner_ros.cpp.

◆ customObstacleCB()

void mpc_local_planner::MpcLocalPlannerROS::customObstacleCB ( const costmap_converter::ObstacleArrayMsg::ConstPtr & obst_msg )

protected

Callback for the dynamic_reconfigure node.

This callback allows to modify parameters dynamically at runtime without restarting the node

Parameters

config	Reference to the dynamic reconfigure config
level	Dynamic reconfigure level

Callback for custom obstacles that are not obtained from the costmap

Parameters

obst_msg pointer to the message containing a list of polygon shaped obstacles

Definition at line 847 of file mpc_local_planner_ros.cpp.

◆ customViaPointsCB()

void mpc_local_planner::MpcLocalPlannerROS::customViaPointsCB ( const nav_msgs::Path::ConstPtr & via_points_msg )

protected

Callback for custom via-points.

Parameters

via_points_msg pointer to the message containing a list of via-points

Definition at line 853 of file mpc_local_planner_ros.cpp.

◆ estimateLocalGoalOrientation()

double mpc_local_planner::MpcLocalPlannerROS::estimateLocalGoalOrientation	(	const std::vector< geometry_msgs::PoseStamped > &	global_plan,
		const geometry_msgs::PoseStamped &	local_goal,
		int	current_goal_idx,
		const geometry_msgs::TransformStamped &	tf_plan_to_global,
		int	moving_average_length = `3`
	)		const

protected

Estimate the orientation of a pose from the global_plan that is treated as a local goal for the local planner.

If the current (local) goal point is not the final one (global) substitute the goal orientation by the angle of the direction vector between the local goal and the subsequent pose of the global plan. This is often helpful, if the global planner does not consider orientations.
A moving average filter is utilized to smooth the orientation.

Parameters

	global_plan	The global plan
	local_goal	Current local goal
	current_goal_idx	Index of the current (local) goal pose in the global plan
[out]	tf_plan_to_global	Transformation between the global plan and the global planning frame
	moving_average_length	number of future poses of the global plan to be taken into account

Returns: orientation (yaw-angle) estimate

Definition at line 791 of file mpc_local_planner_ros.cpp.

◆ getNumberFromXMLRPC()

double mpc_local_planner::MpcLocalPlannerROS::getNumberFromXMLRPC	(	XmlRpc::XmlRpcValue &	value,
		const std::string &	full_param_name
	)

static

Get a number from the given XmlRpcValue.

Remarks: This method is copied from costmap_2d/footprint.h, since it is not declared public in all ros distros; It is modified in order to return a container of Eigen::Vector2d instead of geometry_msgs::Point

Parameters

value	double value type
full_param_name	this is the full name of the rosparam from which the footprint_xmlrpc value came. It is used only for reporting errors.

Returns: double value

Definition at line 1069 of file mpc_local_planner_ros.cpp.

◆ getRobotFootprintFromCostmap2d()

teb_local_planner::RobotFootprintModelPtr mpc_local_planner::MpcLocalPlannerROS::getRobotFootprintFromCostmap2d ( costmap_2d::Costmap2DROS & costmap_ros )

static

Get the current robot footprint/contour model.

Parameters

costmap_ros reference to an intialized instance of costmap_2d::Costmap2dROS

Returns: Robot footprint model used for optimization

Definition at line 1014 of file mpc_local_planner_ros.cpp.

◆ getRobotFootprintFromParamServer()

teb_local_planner::RobotFootprintModelPtr mpc_local_planner::MpcLocalPlannerROS::getRobotFootprintFromParamServer	(	const ros::NodeHandle &	nh,
		costmap_2d::Costmap2DROS *	costmap_ros = `nullptr`
	)

static

Get the current robot footprint/contour model.

Parameters

nh	const reference to the local ros::NodeHandle
costmap_ros	pointer to an intialized instance of costmap_2d::Costmap2dROS

Returns: Robot footprint model used for optimization

Definition at line 874 of file mpc_local_planner_ros.cpp.

◆ initialize()

void mpc_local_planner::MpcLocalPlannerROS::initialize	(	std::string	name,
		tf2_ros::Buffer *	tf,
		costmap_2d::Costmap2DROS *	costmap_ros
	)

virtual

Initializes the teb plugin.

Parameters

name	The name of the instance
tf	Pointer to a tf buffer
costmap_ros	Cost map representing occupied and free space

Implements nav_core::BaseLocalPlanner.

Definition at line 86 of file mpc_local_planner_ros.cpp.

◆ isGoalReached() [1/2]

bool mpc_local_planner::MpcLocalPlannerROS::isGoalReached ( )

virtual

Check if the goal pose has been achieved.

The actual check is performed in computeVelocityCommands(). Only the status flag is checked here.

Returns: True if achieved, false otherwise

Implements nav_core::BaseLocalPlanner.

Definition at line 447 of file mpc_local_planner_ros.cpp.

◆ isGoalReached() [2/2]

bool mpc_local_planner::MpcLocalPlannerROS::isGoalReached	(	double	xy_tolerance,
		double	yaw_tolerance
	)

inlinevirtual

Dummy version to satisfy MBF API.

Implements mbf_costmap_core::CostmapController.

Definition at line 185 of file mpc_local_planner_ros.h.

◆ makeFootprintFromXMLRPC()

teb_local_planner::Point2dContainer mpc_local_planner::MpcLocalPlannerROS::makeFootprintFromXMLRPC	(	XmlRpc::XmlRpcValue &	footprint_xmlrpc,
		const std::string &	full_param_name
	)

static

Set the footprint from the given XmlRpcValue.

Remarks: This method is copied from costmap_2d/footprint.h, since it is not declared public in all ros distros; It is modified in order to return a container of Eigen::Vector2d instead of geometry_msgs::Point

Parameters

footprint_xmlrpc	should be an array of arrays, where the top-level array should have 3 or more elements, and the sub-arrays should all have exactly 2 elements (x and y coordinates).
full_param_name	this is the full name of the rosparam from which the footprint_xmlrpc value came. It is used only for reporting errors.

Returns: container of vertices describing the polygon

Definition at line 1030 of file mpc_local_planner_ros.cpp.

◆ pruneGlobalPlan()

bool mpc_local_planner::MpcLocalPlannerROS::pruneGlobalPlan	(	const tf2_ros::Buffer &	tf,
		const geometry_msgs::PoseStamped &	global_pose,
		std::vector< geometry_msgs::PoseStamped > &	global_plan,
		double	dist_behind_robot = `1`
	)

protected

Prune global plan such that already passed poses are cut off.

The pose of the robot is transformed into the frame of the global plan by taking the most recent tf transform. If no valid transformation can be found, the method returns false. The global plan is pruned until the distance to the robot is at least dist_behind_robot. If no pose within the specified treshold dist_behind_robot can be found, nothing will be pruned and the method returns false.

Remarks: Do not choose dist_behind_robot too small (not smaller the cellsize of the map), otherwise nothing will be pruned.

Parameters

	tf	A reference to a tf buffer
	global_pose	The global pose of the robot
[in,out]	global_plan	The plan to be transformed
	dist_behind_robot	Distance behind the robot that should be kept [meters]

Returns: true if the plan is pruned, false in case of a transform exception or if no pose cannot be found inside the threshold

Definition at line 629 of file mpc_local_planner_ros.cpp.

◆ setPlan()

bool mpc_local_planner::MpcLocalPlannerROS::setPlan ( const std::vector< geometry_msgs::PoseStamped > & orig_global_plan )

virtual

Set the plan that the teb local planner is following.

Parameters

orig_global_plan The plan to pass to the local planner

Returns: True if the plan was updated successfully, false otherwise

Implements mbf_costmap_core::CostmapController.

Definition at line 215 of file mpc_local_planner_ros.cpp.

◆ tfPoseToEigenVector2dTransRot()

Eigen::Vector2d mpc_local_planner::MpcLocalPlannerROS::tfPoseToEigenVector2dTransRot ( const tf::Pose & tf_vel )

static

Transform a tf::Pose type into a Eigen::Vector2d containing the translational and angular velocities.

Translational velocities (x- and y-coordinates) are combined into a single translational velocity (first component).

Parameters

tf_vel tf::Pose message containing a 1D or 2D translational velocity (x,y) and an angular velocity (yaw-angle)

Returns: Translational and angular velocity combined into an Eigen::Vector2d

Definition at line 621 of file mpc_local_planner_ros.cpp.

◆ transformGlobalPlan()

bool mpc_local_planner::MpcLocalPlannerROS::transformGlobalPlan	(	const tf2_ros::Buffer &	tf,
		const std::vector< geometry_msgs::PoseStamped > &	global_plan,
		const geometry_msgs::PoseStamped &	global_pose,
		const costmap_2d::Costmap2D &	costmap,
		const std::string &	global_frame,
		double	max_plan_length,
		std::vector< geometry_msgs::PoseStamped > &	transformed_plan,
		int *	current_goal_idx = `NULL`,
		geometry_msgs::TransformStamped *	tf_plan_to_global = `NULL`
	)		const

protected

Transforms the global plan of the robot from the planner frame to the local frame (modified).

The method replaces transformGlobalPlan as defined in base_local_planner/goal_functions.h such that the index of the current goal pose is returned as well as the transformation between the global plan and the planning frame.

Parameters

	tf	A reference to a tf buffer
	global_plan	The plan to be transformed
	global_pose	The global pose of the robot
	costmap	A reference to the costmap being used so the window size for transforming can be computed
	global_frame	The frame to transform the plan to
	max_plan_length	Specify maximum length (cumulative Euclidean distances) of the transformed plan [if <=0: disabled; the length is also bounded by the local costmap size!]
[out]	transformed_plan	Populated with the transformed plan
[out]	current_goal_idx	Index of the current (local) goal pose in the global plan
[out]	tf_plan_to_global	Transformation between the global plan and the global planning frame

Returns: true if the global plan is transformed, false otherwise

Definition at line 671 of file mpc_local_planner_ros.cpp.

◆ updateObstacleContainerWithCostmap()

void mpc_local_planner::MpcLocalPlannerROS::updateObstacleContainerWithCostmap ( )

protected

Update internal obstacle vector based on occupied costmap cells.

Remarks: All occupied cells will be added as point obstacles.; All previous obstacles are cleared.

See also: updateObstacleContainerWithCostmapConverter

Todo:

Include temporal coherence among obstacle msgs (id vector)

Include properties for dynamic obstacles (e.g. using constant velocity model)

Definition at line 458 of file mpc_local_planner_ros.cpp.

◆ updateObstacleContainerWithCostmapConverter()

void mpc_local_planner::MpcLocalPlannerROS::updateObstacleContainerWithCostmapConverter ( )

protected

Update internal obstacle vector based on polygons provided by a costmap_converter plugin.

Remarks: Requires a loaded costmap_converter plugin.; All previous obstacles are cleared.

See also: updateObstacleContainerWithCostmap

Definition at line 485 of file mpc_local_planner_ros.cpp.

◆ updateObstacleContainerWithCustomObstacles()

void mpc_local_planner::MpcLocalPlannerROS::updateObstacleContainerWithCustomObstacles ( )

protected

Update internal obstacle vector based on custom messages received via subscriber.

Remarks: All previous obstacles are NOT cleared. Call this method after other update methods.

See also: updateObstacleContainerWithCostmap, updateObstacleContainerWithCostmapConverter

Definition at line 527 of file mpc_local_planner_ros.cpp.

◆ updateViaPointsContainer()

void mpc_local_planner::MpcLocalPlannerROS::updateViaPointsContainer	(	const std::vector< geometry_msgs::PoseStamped > &	transformed_plan,
		double	min_separation
	)

protected

Update internal via-point container based on the current reference plan.

Remarks: All previous via-points will be cleared.

Parameters

transformed_plan	(local) portion of the global plan (which is already transformed to the planning frame)
min_separation	minimum separation between two consecutive via-points

Definition at line 603 of file mpc_local_planner_ros.cpp.

◆ validateFootprints()

void mpc_local_planner::MpcLocalPlannerROS::validateFootprints	(	double	opt_inscribed_radius,
		double	costmap_inscribed_radius,
		double	min_obst_dist
	)

protected

Validate current parameter values of the footprint for optimization, obstacle distance and the costmap footprint.

This method prints warnings if validation fails.

Remarks: Currently, we only validate the inscribed radius of the footprints

Parameters

opt_inscribed_radius	Inscribed radius of the RobotFootprintModel for optimization
costmap_inscribed_radius	Inscribed radius of the footprint model used for the costmap
min_obst_dist	desired distance to obstacles

Definition at line 838 of file mpc_local_planner_ros.cpp.

Member Data Documentation

◆ _controller

Controller mpc_local_planner::MpcLocalPlannerROS::_controller

private

Definition at line 381 of file mpc_local_planner_ros.h.

◆ _costmap

costmap_2d::Costmap2D* mpc_local_planner::MpcLocalPlannerROS::_costmap

private

Pointer to the 2d costmap (obtained from the costmap ros wrapper)

Definition at line 377 of file mpc_local_planner_ros.h.

◆ _costmap_conv_params

struct mpc_local_planner::MpcLocalPlannerROS::CostmapConverterPlugin mpc_local_planner::MpcLocalPlannerROS::_costmap_conv_params

private

◆ _costmap_converter

boost::shared_ptr<costmap_converter::BaseCostmapToPolygons> mpc_local_planner::MpcLocalPlannerROS::_costmap_converter

private

Store the current costmap_converter.

Definition at line 394 of file mpc_local_planner_ros.h.

◆ _costmap_converter_loader

pluginlib::ClassLoader<costmap_converter::BaseCostmapToPolygons> mpc_local_planner::MpcLocalPlannerROS::_costmap_converter_loader

private

Load costmap converter plugins at runtime.

Definition at line 393 of file mpc_local_planner_ros.h.

◆ _costmap_model

std::shared_ptr<base_local_planner::CostmapModel> mpc_local_planner::MpcLocalPlannerROS::_costmap_model

private

Definition at line 384 of file mpc_local_planner_ros.h.

◆ _costmap_ros

costmap_2d::Costmap2DROS* mpc_local_planner::MpcLocalPlannerROS::_costmap_ros

private

Pointer to the costmap ros wrapper, received from the navigation stack.

Definition at line 376 of file mpc_local_planner_ros.h.

◆ _custom_obst_mutex

std::mutex mpc_local_planner::MpcLocalPlannerROS::_custom_obst_mutex

private

Mutex that locks the obstacle array (multi-threaded)

Definition at line 399 of file mpc_local_planner_ros.h.

◆ _custom_obst_sub

ros::Subscriber mpc_local_planner::MpcLocalPlannerROS::_custom_obst_sub

private

Subscriber for custom obstacles received via a ObstacleMsg.

Definition at line 398 of file mpc_local_planner_ros.h.

◆ _custom_obstacle_msg

costmap_converter::ObstacleArrayMsg mpc_local_planner::MpcLocalPlannerROS::_custom_obstacle_msg

private

Copy of the most recent obstacle message.

Definition at line 400 of file mpc_local_planner_ros.h.

◆ _custom_via_points_active

bool mpc_local_planner::MpcLocalPlannerROS::_custom_via_points_active = false

private

Keep track whether valid via-points have been received from via_points_sub_.

Definition at line 404 of file mpc_local_planner_ros.h.

◆ _footprint_spec

std::vector<geometry_msgs::Point> mpc_local_planner::MpcLocalPlannerROS::_footprint_spec

private

Store the footprint of the robot.

Definition at line 418 of file mpc_local_planner_ros.h.

◆ _global_frame

std::string mpc_local_planner::MpcLocalPlannerROS::_global_frame

private

The frame in which the controller will run.

Definition at line 422 of file mpc_local_planner_ros.h.

◆ _global_plan

std::vector<geometry_msgs::PoseStamped> mpc_local_planner::MpcLocalPlannerROS::_global_plan

private

Store the current global plan.

Definition at line 389 of file mpc_local_planner_ros.h.

◆ _goal_reached

bool mpc_local_planner::MpcLocalPlannerROS::_goal_reached = false

private

store whether the goal is reached or not

Definition at line 410 of file mpc_local_planner_ros.h.

◆ _initialized

bool mpc_local_planner::MpcLocalPlannerROS::_initialized

private

Keeps track about the correct initialization of this class.

Definition at line 426 of file mpc_local_planner_ros.h.

◆ _last_cmd

geometry_msgs::Twist mpc_local_planner::MpcLocalPlannerROS::_last_cmd

private

Store the last control command generated in computeVelocityCommands()

Definition at line 413 of file mpc_local_planner_ros.h.

◆ _no_infeasible_plans

int mpc_local_planner::MpcLocalPlannerROS::_no_infeasible_plans = 0

private

Store how many times in a row the planner failed to find a feasible plan.

Definition at line 412 of file mpc_local_planner_ros.h.

◆ _obstacles

ObstContainer mpc_local_planner::MpcLocalPlannerROS::_obstacles

private

Obstacle vector that should be considered during local trajectory optimization.

Definition at line 382 of file mpc_local_planner_ros.h.

◆ _odom_helper

base_local_planner::OdometryHelperRos mpc_local_planner::MpcLocalPlannerROS::_odom_helper

private

Provides an interface to receive the current velocity from the robot.

Definition at line 391 of file mpc_local_planner_ros.h.

◆ _params

struct mpc_local_planner::MpcLocalPlannerROS::Parameters mpc_local_planner::MpcLocalPlannerROS::_params

private

◆ _publisher

Publisher mpc_local_planner::MpcLocalPlannerROS::_publisher

private

Definition at line 383 of file mpc_local_planner_ros.h.

◆ _robot_base_frame

std::string mpc_local_planner::MpcLocalPlannerROS::_robot_base_frame

private

Used as the base frame id of the robot.

Definition at line 423 of file mpc_local_planner_ros.h.

◆ _robot_circumscribed_radius

double mpc_local_planner::MpcLocalPlannerROS::_robot_circumscribed_radius

private

The radius of the circumscribed circle of the robot.

Definition at line 420 of file mpc_local_planner_ros.h.

◆ _robot_goal

PoseSE2 mpc_local_planner::MpcLocalPlannerROS::_robot_goal

private

Store current robot goal.

Definition at line 408 of file mpc_local_planner_ros.h.

◆ _robot_inscribed_radius

double mpc_local_planner::MpcLocalPlannerROS::_robot_inscribed_radius

private

The radius of the inscribed circle of the robot (collision possible)

Definition at line 419 of file mpc_local_planner_ros.h.

◆ _robot_model

RobotFootprintModelPtr mpc_local_planner::MpcLocalPlannerROS::_robot_model

private

Definition at line 416 of file mpc_local_planner_ros.h.

◆ _robot_pose

PoseSE2 mpc_local_planner::MpcLocalPlannerROS::_robot_pose

private

Store current robot pose.

Definition at line 407 of file mpc_local_planner_ros.h.

◆ _robot_vel

geometry_msgs::Twist mpc_local_planner::MpcLocalPlannerROS::_robot_vel

private

Store current robot translational and angular velocity (vx, vy, omega)

Definition at line 409 of file mpc_local_planner_ros.h.

◆ _tf

tf2_ros::Buffer* mpc_local_planner::MpcLocalPlannerROS::_tf

private

pointer to tf buffer

Definition at line 378 of file mpc_local_planner_ros.h.

◆ _time_last_cmd

ros::Time mpc_local_planner::MpcLocalPlannerROS::_time_last_cmd

private

Definition at line 414 of file mpc_local_planner_ros.h.

◆ _time_last_infeasible_plan

ros::Time mpc_local_planner::MpcLocalPlannerROS::_time_last_infeasible_plan

private

Store at which time stamp the last infeasible plan was detected.

Definition at line 411 of file mpc_local_planner_ros.h.

◆ _u_seq

corbo::TimeSeries::Ptr mpc_local_planner::MpcLocalPlannerROS::_u_seq = std::make_shared<corbo::TimeSeries>()

private

Definition at line 387 of file mpc_local_planner_ros.h.

◆ _via_point_mutex

std::mutex mpc_local_planner::MpcLocalPlannerROS::_via_point_mutex

private

Mutex that locks the via_points container (multi-threaded)

Definition at line 405 of file mpc_local_planner_ros.h.

◆ _via_points

ViaPointContainer mpc_local_planner::MpcLocalPlannerROS::_via_points

private

Definition at line 402 of file mpc_local_planner_ros.h.

◆ _via_points_sub

ros::Subscriber mpc_local_planner::MpcLocalPlannerROS::_via_points_sub

private

Subscriber for custom via-points received via a Path msg.

Definition at line 403 of file mpc_local_planner_ros.h.

◆ _x_seq

corbo::TimeSeries::Ptr mpc_local_planner::MpcLocalPlannerROS::_x_seq = std::make_shared<corbo::TimeSeries>()

private

Definition at line 386 of file mpc_local_planner_ros.h.

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

Classes

Public Member Functions

Private Types

Public utility functions/methods

Additional Inherited Members

Detailed Description

Member Typedef Documentation

◆ CircularObstacle

◆ LineObstacle

◆ ObstaclePtr

◆ ObstContainer

◆ Point2dContainer

◆ PointObstacle

◆ PolygonObstacle

◆ PoseSE2

◆ RobotFootprintModelPtr

◆ ViaPointContainer

Constructor & Destructor Documentation

◆ MpcLocalPlannerROS()

◆ ~MpcLocalPlannerROS()

Member Function Documentation

◆ cancel()

◆ computeVelocityCommands() [1/2]

◆ computeVelocityCommands() [2/2]

◆ customObstacleCB()

◆ customViaPointsCB()

◆ estimateLocalGoalOrientation()

◆ getNumberFromXMLRPC()

◆ getRobotFootprintFromCostmap2d()

◆ getRobotFootprintFromParamServer()

◆ initialize()

◆ isGoalReached() [1/2]

◆ isGoalReached() [2/2]

◆ makeFootprintFromXMLRPC()

◆ pruneGlobalPlan()

◆ setPlan()

◆ tfPoseToEigenVector2dTransRot()

◆ transformGlobalPlan()

◆ updateObstacleContainerWithCostmap()

◆ updateObstacleContainerWithCostmapConverter()

◆ updateObstacleContainerWithCustomObstacles()

◆ updateViaPointsContainer()

◆ validateFootprints()

Member Data Documentation

◆ _controller

◆ _costmap

◆ _costmap_conv_params

◆ _costmap_converter

◆ _costmap_converter_loader

◆ _costmap_model

◆ _costmap_ros

◆ _custom_obst_mutex

◆ _custom_obst_sub

◆ _custom_obstacle_msg

◆ _custom_via_points_active

◆ _footprint_spec

◆ _global_frame

◆ _global_plan

◆ _goal_reached

◆ _initialized

◆ _last_cmd

◆ _no_infeasible_plans

◆ _obstacles

◆ _odom_helper

◆ _params

◆ _publisher

◆ _robot_base_frame

◆ _robot_circumscribed_radius

◆ _robot_goal

◆ _robot_inscribed_radius

◆ _robot_model

◆ _robot_pose

◆ _robot_vel

◆ _tf

◆ _time_last_cmd

◆ _time_last_infeasible_plan

◆ _u_seq

◆ _via_point_mutex

◆ _via_points

◆ _via_points_sub