MPC controller for mobile robots. More...

#include <controller.h>

Inheritance diagram for mpc_local_planner::Controller:

Public Types
using	PoseSE2 = teb_local_planner::PoseSE2

using	Ptr = std::shared_ptr< Controller >

Public Types inherited from corbo::PredictiveController
typedef std::shared_ptr< PredictiveController >	Ptr

Public Types inherited from corbo::ControllerInterface
typedef Eigen::VectorXd	ControlVector

typedef std::shared_ptr< ControllerInterface >	Ptr

typedef Eigen::VectorXd	StateVector

typedef std::unique_ptr< ControllerInterface >	UPtr

Public Member Functions
bool	configure (ros::NodeHandle &nh, const teb_local_planner::ObstContainer &obstacles, teb_local_planner::RobotFootprintModelPtr robot_model, const std::vector< teb_local_planner::PoseSE2 > &via_points)

	Controller ()=default

StageInequalitySE2::Ptr	getInequalityConstraint ()

corbo::ControllerInterface::Ptr	getInstance () const override

RobotDynamicsInterface::Ptr	getRobotDynamics ()

virtual bool	isPoseTrajectoryFeasible (base_local_planner::CostmapModel *costmap_model, const std::vector< geometry_msgs::Point > &footprint_spec, double inscribed_radius=0.0, double circumscribed_radius=0.0, double min_resolution_collision_check_angular=M_PI, int look_ahead_idx=-1)
	Check whether the planned trajectory is feasible or not. More...

void	publishOptimalControlResult ()

void	reset () override

void	setInitialPlanEstimateOrientation (bool estimate)

void	setOptimalControlProblem (corbo::OptimalControlProblemInterface::Ptr ocp)=delete

void	stateFeedbackCallback (const mpc_local_planner_msgs::StateFeedback::ConstPtr &msg)

bool	step (const PoseSE2 &start, const PoseSE2 &goal, const geometry_msgs::Twist &vel, double dt, ros::Time t, corbo::TimeSeries::Ptr u_seq, corbo::TimeSeries::Ptr x_seq)

bool	step (const std::vector< geometry_msgs::PoseStamped > &initial_plan, const geometry_msgs::Twist &vel, double dt, ros::Time t, corbo::TimeSeries::Ptr u_seq, corbo::TimeSeries::Ptr x_seq)

Public Member Functions inherited from corbo::PredictiveController
bool	getAutoUpdatePreviousControl () const

void	getAvailableSignals (SignalTargetInterface &signal_target, const std::string &ns="") const override

double	getControlDuration () const override

int	getControlInputDimension () const override

const int &	getNumOcpIterations () const

OptimalControlProblemInterface::Ptr	getOptimalControlProblem ()

int	getStateDimension () const override

ControllerStatistics::Ptr	getStatistics () const override

bool	hasPiecewiseConstantControls () const override

bool	initialize (const StateVector &x, ReferenceTrajectoryInterface &expected_xref, ReferenceTrajectoryInterface &expected_uref, const Duration &expected_dt, const Time &t, ReferenceTrajectoryInterface *sref=nullptr) override

const bool &	isPublishPrediction () const

	PredictiveController ()

bool	providesFutureControls () const override

bool	providesFutureStates () const override

void	sendSignals (double t, SignalTargetInterface &signal_target, const std::string &ns="") const override

void	setAutoUpdatePreviousControl (bool enable)

void	setNumOcpIterations (int ocp_iter)

void	setOptimalControlProblem (OptimalControlProblemInterface::Ptr ocp)

void	setOutputControlSequenceLenght (bool activate)

void	setOutputStateSequenceLenght (bool activate)

void	setPublishPrediction (bool publish)

bool	step (const StateVector &x, ReferenceTrajectoryInterface &xref, ReferenceTrajectoryInterface &uref, const Duration &dt, const Time &t, TimeSeries::Ptr u_sequence, TimeSeries::Ptr x_sequence, SignalTargetInterface signal_target=nullptr, ReferenceTrajectoryInterface sref=nullptr, ReferenceTrajectoryInterface xinit=nullptr, ReferenceTrajectoryInterface uinit=nullptr, const std::string &ns="") override

bool	supportsAsynchronousControl () const override

Public Member Functions inherited from corbo::ControllerInterface
virtual bool	step (const StateVector &x, ReferenceTrajectoryInterface &xref, ReferenceTrajectoryInterface &uref, const Duration &dt, const Time &t, ControlVector &u, SignalTargetInterface *signal_target=nullptr, const std::string &ns="")

virtual	~ControllerInterface ()

Static Public Member Functions
static corbo::ControllerInterface::Ptr	getInstanceStatic ()

Static Public Member Functions inherited from corbo::PredictiveController
static Ptr	getInstanceStatic ()

Static Public Member Functions inherited from corbo::ControllerInterface
static Factory< ControllerInterface > &	getFactory ()

Protected Member Functions
corbo::DiscretizationGridInterface::Ptr	configureGrid (const ros::NodeHandle &nh)

corbo::StructuredOptimalControlProblem::Ptr	configureOcp (const ros::NodeHandle &nh, const teb_local_planner::ObstContainer &obstacles, teb_local_planner::RobotFootprintModelPtr robot_model, const std::vector< teb_local_planner::PoseSE2 > &via_points)

RobotDynamicsInterface::Ptr	configureRobotDynamics (const ros::NodeHandle &nh)

corbo::NlpSolverInterface::Ptr	configureSolver (const ros::NodeHandle &nh)

bool	generateInitialStateTrajectory (const Eigen::VectorXd &x0, const Eigen::VectorXd &xf, const std::vector< geometry_msgs::PoseStamped > &initial_plan, bool backward)

Protected Attributes
RobotDynamicsInterface::Ptr	_dynamics

double	_force_reinit_new_goal_angular = 0.5 * M_PI

double	_force_reinit_new_goal_dist = 1.0

int	_force_reinit_num_steps = 0

corbo::DiscretizationGridInterface::Ptr	_grid

bool	_guess_backwards_motion = true

StageInequalitySE2::Ptr	_inequality_constraint

bool	_initial_plan_estimate_orientation = true

teb_local_planner::PoseSE2	_last_goal

ros::Publisher	_ocp_result_pub

std::size_t	_ocp_seq = 0

bool	_ocp_successful = false

bool	_prefer_x_feedback = false

bool	_print_cpu_time = false

bool	_publish_ocp_results = false

Eigen::VectorXd	_recent_x_feedback

ros::Time	_recent_x_time

std::string	_robot_type

corbo::NlpSolverInterface::Ptr	_solver

corbo::StructuredOptimalControlProblem::Ptr	_structured_ocp

std::mutex	_x_feedback_mutex

ros::Subscriber	_x_feedback_sub

corbo::DiscreteTimeReferenceTrajectory	_x_seq_init

Protected Attributes inherited from corbo::PredictiveController
bool	_auto_update_prev_control

bool	_initialized

int	_num_ocp_iterations

OptimalControlProblemInterface::Ptr	_ocp

bool	_output_control_sequence

bool	_output_state_sequence

bool	_publish_prediction

ControllerStatistics	_statistics

TimeSeries::Ptr	_u_ts

TimeSeries::Ptr	_x_ts

Detailed Description

MPC controller for mobile robots.

Author: Christoph Rösmann (chris.nosp@m.toph.nosp@m..roes.nosp@m.mann.nosp@m.@tu-d.nosp@m.ortm.nosp@m.und.d.nosp@m.e)

Definition at line 53 of file controller.h.

Member Typedef Documentation

◆ PoseSE2

using mpc_local_planner::Controller::PoseSE2 = teb_local_planner::PoseSE2

Definition at line 57 of file controller.h.

◆ Ptr

using mpc_local_planner::Controller::Ptr = std::shared_ptr<Controller>

Definition at line 56 of file controller.h.

Constructor & Destructor Documentation

◆ Controller()

mpc_local_planner::Controller::Controller ( )

default

Member Function Documentation

◆ configure()

bool mpc_local_planner::Controller::configure	(	ros::NodeHandle &	nh,
		const teb_local_planner::ObstContainer &	obstacles,
		teb_local_planner::RobotFootprintModelPtr	robot_model,
		const std::vector< teb_local_planner::PoseSE2 > &	via_points
	)

Definition at line 58 of file controller.cpp.

◆ configureGrid()

corbo::DiscretizationGridInterface::Ptr mpc_local_planner::Controller::configureGrid ( const ros::NodeHandle & nh )

protected

Definition at line 225 of file controller.cpp.

◆ configureOcp()

corbo::StructuredOptimalControlProblem::Ptr mpc_local_planner::Controller::configureOcp	(	const ros::NodeHandle &	nh,
		const teb_local_planner::ObstContainer &	obstacles,
		teb_local_planner::RobotFootprintModelPtr	robot_model,
		const std::vector< teb_local_planner::PoseSE2 > &	via_points
	)

protected

Definition at line 483 of file controller.cpp.

◆ configureRobotDynamics()

RobotDynamicsInterface::Ptr mpc_local_planner::Controller::configureRobotDynamics ( const ros::NodeHandle & nh )

protected

Definition at line 344 of file controller.cpp.

◆ configureSolver()

corbo::NlpSolverInterface::Ptr mpc_local_planner::Controller::configureSolver ( const ros::NodeHandle & nh )

protected

Definition at line 380 of file controller.cpp.

◆ generateInitialStateTrajectory()

bool mpc_local_planner::Controller::generateInitialStateTrajectory	(	const Eigen::VectorXd &	x0,
		const Eigen::VectorXd &	xf,
		const std::vector< geometry_msgs::PoseStamped > &	initial_plan,
		bool	backward
	)

protected

Definition at line 807 of file controller.cpp.

◆ getInequalityConstraint()

StageInequalitySE2::Ptr mpc_local_planner::Controller::getInequalityConstraint ( )

inline

Definition at line 76 of file controller.h.

◆ getInstance()

corbo::ControllerInterface::Ptr mpc_local_planner::Controller::getInstance ( ) const

inlineoverridevirtual

Reimplemented from corbo::PredictiveController.

Definition at line 70 of file controller.h.

◆ getInstanceStatic()

static corbo::ControllerInterface::Ptr mpc_local_planner::Controller::getInstanceStatic ( )

inlinestatic

Definition at line 71 of file controller.h.

◆ getRobotDynamics()

RobotDynamicsInterface::Ptr mpc_local_planner::Controller::getRobotDynamics ( )

inline

Definition at line 75 of file controller.h.

◆ isPoseTrajectoryFeasible()

bool mpc_local_planner::Controller::isPoseTrajectoryFeasible	(	base_local_planner::CostmapModel *	costmap_model,
		const std::vector< geometry_msgs::Point > &	footprint_spec,
		double	inscribed_radius = `0.0`,
		double	circumscribed_radius = `0.0`,
		double	min_resolution_collision_check_angular = `M_PI`,
		int	look_ahead_idx = `-1`
	)

virtual

Check whether the planned trajectory is feasible or not.

This method currently checks only that the trajectory, or a part of the trajectory is collision free. Obstacles are here represented as costmap instead of the internal ObstacleContainer.

Parameters

costmap_model	Pointer to the costmap model
footprint_spec	The specification of the footprint of the robot in world coordinates
inscribed_radius	The radius of the inscribed circle of the robot
circumscribed_radius	The radius of the circumscribed circle of the robot
min_resolution_collision_check_angular	Min angular resolution during the costmap collision check: if not respected intermediate samples are added. [rad]
look_ahead_idx	Number of poses along the trajectory that should be verified, if -1, the complete trajectory will be checked.

Returns: true, if the robot footprint along the first part of the trajectory intersects with any obstacle in the costmap, false otherwise.

Definition at line 859 of file controller.cpp.

◆ publishOptimalControlResult()

void mpc_local_planner::Controller::publishOptimalControlResult ( )

Definition at line 197 of file controller.cpp.

◆ reset()

void mpc_local_planner::Controller::reset ( )

overridevirtual

Reimplemented from corbo::PredictiveController.

Definition at line 223 of file controller.cpp.

◆ setInitialPlanEstimateOrientation()

void mpc_local_planner::Controller::setInitialPlanEstimateOrientation ( bool estimate )

inline

Definition at line 82 of file controller.h.

◆ setOptimalControlProblem()

void mpc_local_planner::Controller::setOptimalControlProblem ( corbo::OptimalControlProblemInterface::Ptr ocp )

delete

◆ stateFeedbackCallback()

void mpc_local_planner::Controller::stateFeedbackCallback ( const mpc_local_planner_msgs::StateFeedback::ConstPtr & msg )

Definition at line 181 of file controller.cpp.

◆ step() [1/2]

bool mpc_local_planner::Controller::step	(	const PoseSE2 &	start,
		const PoseSE2 &	goal,
		const geometry_msgs::Twist &	vel,
		double	dt,
		ros::Time	t,
		corbo::TimeSeries::Ptr	u_seq,
		corbo::TimeSeries::Ptr	x_seq
	)

Definition at line 102 of file controller.cpp.

◆ step() [2/2]

bool mpc_local_planner::Controller::step	(	const std::vector< geometry_msgs::PoseStamped > &	initial_plan,
		const geometry_msgs::Twist &	vel,
		double	dt,
		ros::Time	t,
		corbo::TimeSeries::Ptr	u_seq,
		corbo::TimeSeries::Ptr	x_seq
	)

Definition at line 111 of file controller.cpp.

Member Data Documentation

◆ _dynamics

RobotDynamicsInterface::Ptr mpc_local_planner::Controller::_dynamics

protected

Definition at line 119 of file controller.h.

◆ _force_reinit_new_goal_angular

double mpc_local_planner::Controller::_force_reinit_new_goal_angular = 0.5 * M_PI

protected

Definition at line 138 of file controller.h.

◆ _force_reinit_new_goal_dist

double mpc_local_planner::Controller::_force_reinit_new_goal_dist = 1.0

protected

Definition at line 137 of file controller.h.

◆ _force_reinit_num_steps

int mpc_local_planner::Controller::_force_reinit_num_steps = 0

protected

Definition at line 142 of file controller.h.

◆ _grid

corbo::DiscretizationGridInterface::Ptr mpc_local_planner::Controller::_grid

protected

Definition at line 118 of file controller.h.

◆ _guess_backwards_motion

bool mpc_local_planner::Controller::_guess_backwards_motion = true

protected

Definition at line 141 of file controller.h.

◆ _inequality_constraint

StageInequalitySE2::Ptr mpc_local_planner::Controller::_inequality_constraint

protected

Definition at line 121 of file controller.h.

◆ _initial_plan_estimate_orientation

bool mpc_local_planner::Controller::_initial_plan_estimate_orientation = true

protected

Definition at line 140 of file controller.h.

◆ _last_goal

teb_local_planner::PoseSE2 mpc_local_planner::Controller::_last_goal

protected

Definition at line 136 of file controller.h.

◆ _ocp_result_pub

ros::Publisher mpc_local_planner::Controller::_ocp_result_pub

protected

Definition at line 124 of file controller.h.

◆ _ocp_seq

std::size_t mpc_local_planner::Controller::_ocp_seq = 0

protected

Definition at line 126 of file controller.h.

◆ _ocp_successful

bool mpc_local_planner::Controller::_ocp_successful = false

protected

Definition at line 125 of file controller.h.

◆ _prefer_x_feedback

bool mpc_local_planner::Controller::_prefer_x_feedback = false

protected

Definition at line 130 of file controller.h.

◆ _print_cpu_time

bool mpc_local_planner::Controller::_print_cpu_time = false

protected

Definition at line 128 of file controller.h.

◆ _publish_ocp_results

bool mpc_local_planner::Controller::_publish_ocp_results = false

protected

Definition at line 127 of file controller.h.

◆ _recent_x_feedback

Eigen::VectorXd mpc_local_planner::Controller::_recent_x_feedback

protected

Definition at line 134 of file controller.h.

◆ _recent_x_time

ros::Time mpc_local_planner::Controller::_recent_x_time

protected

Definition at line 133 of file controller.h.

◆ _robot_type

std::string mpc_local_planner::Controller::_robot_type

protected

Definition at line 117 of file controller.h.

◆ _solver

corbo::NlpSolverInterface::Ptr mpc_local_planner::Controller::_solver

protected

Definition at line 120 of file controller.h.

◆ _structured_ocp

corbo::StructuredOptimalControlProblem::Ptr mpc_local_planner::Controller::_structured_ocp

protected

Definition at line 122 of file controller.h.

◆ _x_feedback_mutex

std::mutex mpc_local_planner::Controller::_x_feedback_mutex

protected

Definition at line 132 of file controller.h.

◆ _x_feedback_sub

ros::Subscriber mpc_local_planner::Controller::_x_feedback_sub

protected

Definition at line 131 of file controller.h.

◆ _x_seq_init

corbo::DiscreteTimeReferenceTrajectory mpc_local_planner::Controller::_x_seq_init

protected

Definition at line 139 of file controller.h.

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

Public Types

Public Member Functions

Static Public Member Functions

Protected Member Functions

Protected Attributes

Detailed Description

Member Typedef Documentation

◆ PoseSE2

◆ Ptr

Constructor & Destructor Documentation

◆ Controller()

Member Function Documentation

◆ configure()

◆ configureGrid()

◆ configureOcp()

◆ configureRobotDynamics()

◆ configureSolver()

◆ generateInitialStateTrajectory()

◆ getInequalityConstraint()

◆ getInstance()

◆ getInstanceStatic()

◆ getRobotDynamics()

◆ isPoseTrajectoryFeasible()

◆ publishOptimalControlResult()

◆ reset()

◆ setInitialPlanEstimateOrientation()

◆ setOptimalControlProblem()

◆ stateFeedbackCallback()

◆ step() [1/2]

◆ step() [2/2]

Member Data Documentation

◆ _dynamics

◆ _force_reinit_new_goal_angular

◆ _force_reinit_new_goal_dist

◆ _force_reinit_num_steps

◆ _grid

◆ _guess_backwards_motion

◆ _inequality_constraint

◆ _initial_plan_estimate_orientation

◆ _last_goal

◆ _ocp_result_pub

◆ _ocp_seq

◆ _ocp_successful

◆ _prefer_x_feedback

◆ _print_cpu_time

◆ _publish_ocp_results

◆ _recent_x_feedback

◆ _recent_x_time

◆ _robot_type

◆ _solver

◆ _structured_ocp

◆ _x_feedback_mutex

◆ _x_feedback_sub

◆ _x_seq_init