Todo List

Member mpc_local_planner::convert (const corbo::TimeSeries &time_series, const RobotDynamicsInterface &dynamics, std::vector< geometry_msgs::PoseStamped > &poses_stamped, const std::string &frame_id)

We could avoid the system dynamics dependency by specifying a generic getter function for the SE2 poses

Member mpc_local_planner::FullDiscretizationGridBaseSE2::resampleTrajectory (int n_new)

(roesmann) More efficient strategy without copying containers at all?

Class mpc_local_planner::MinTimeViaPointsCost

we can implement this class as LSQ version as well

Class mpc_local_planner::MpcLocalPlannerROS

Escape behavior, more efficient obstacle handling

Member mpc_local_planner::MpcLocalPlannerROS::updateObstacleContainerWithCostmap ()

Include temporal coherence among obstacle msgs (id vector)

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

Member mpc_local_planner::Publisher::publishObstacles (const teb_local_planner::ObstContainer &obstacles) const

Move filling of the marker message to polygon class in order to avoid checking types.

Member mpc_local_planner::Publisher::publishViaPoints (const std::vector< teb_local_planner::PoseSE2 > &via_points, const std::string &ns="ViaPoints") const

add option to switch between points and poses (including orientation) to be published

Member mpc_local_planner::RobotDynamicsInterface::getTwistFromControl (const Eigen::Ref< const Eigen::VectorXd > &u, geometry_msgs::Twist &twist) const =0

Maybe add current state x as optional input to allow for computing a twist out of the first state and control