diff --git a/mpc_local_planner/cfg/test_mpc_optim_node.yaml b/mpc_local_planner/cfg/test_mpc_optim_node.yaml index d600e34..62e50d6 100644 --- a/mpc_local_planner/cfg/test_mpc_optim_node.yaml +++ b/mpc_local_planner/cfg/test_mpc_optim_node.yaml @@ -36,6 +36,7 @@ collision_avoidance: enable_dynamic_obstacles: False force_inclusion_dist: 0.5 cutoff_dist: 2.5 + collision_check_no_poses: 5 ## Planning grid grid: diff --git a/mpc_local_planner/include/mpc_local_planner/controller.h b/mpc_local_planner/include/mpc_local_planner/controller.h index 3ee540a..d39c012 100644 --- a/mpc_local_planner/include/mpc_local_planner/controller.h +++ b/mpc_local_planner/include/mpc_local_planner/controller.h @@ -33,6 +33,8 @@ #include +#include + #include #include @@ -79,6 +81,25 @@ class Controller : public corbo::PredictiveController void setInitialPlanEstimateOrientation(bool estimate) { _initial_plan_estimate_orientation = estimate; } + /** + * @brief 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. + * @param costmap_model Pointer to the costmap model + * @param footprint_spec The specification of the footprint of the robot in world coordinates + * @param inscribed_radius The radius of the inscribed circle of the robot + * @param circumscribed_radius The radius of the circumscribed circle of the robot + * @param min_resolution_collision_check_angular Min angular resolution during the costmap collision check: + * if not respected intermediate samples are added. [rad] + * @param look_ahead_idx Number of poses along the trajectory that should be verified, if -1, the complete trajectory will be checked. + * @return \c true, if the robot footprint along the first part of the trajectory intersects with + * any obstacle in the costmap, \c false otherwise. + */ + virtual bool isPoseTrajectoryFeasible(base_local_planner::CostmapModel* costmap_model, const std::vector& 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); + // implements interface method void reset() override; diff --git a/mpc_local_planner/include/mpc_local_planner/mpc_local_planner_ros.h b/mpc_local_planner/include/mpc_local_planner/mpc_local_planner_ros.h index ab60bce..1839955 100644 --- a/mpc_local_planner/include/mpc_local_planner/mpc_local_planner_ros.h +++ b/mpc_local_planner/include/mpc_local_planner/mpc_local_planner_ros.h @@ -407,17 +407,19 @@ class MpcLocalPlannerROS : public nav_core::BaseLocalPlanner, public mbf_costmap struct Parameters { - double xy_goal_tolerance = 0.2; - double yaw_goal_tolerance = 0.1; - bool global_plan_overwrite_orientation = true; - double global_plan_prune_distance = 1.0; - double max_global_plan_lookahead_dist = 1.5; - bool is_footprint_dynamic = false; - bool include_costmap_obstacles = true; - double costmap_obstacles_behind_robot_dist = 1.5; - double global_plan_viapoint_sep = -1; - std::string odom_topic = "odom"; - double controller_frequency = 10; + double xy_goal_tolerance = 0.2; + double yaw_goal_tolerance = 0.1; + bool global_plan_overwrite_orientation = true; + double global_plan_prune_distance = 1.0; + double max_global_plan_lookahead_dist = 1.5; + bool is_footprint_dynamic = false; + bool include_costmap_obstacles = true; + double costmap_obstacles_behind_robot_dist = 1.5; + double global_plan_viapoint_sep = -1; + double collision_check_min_resolution_angular = M_PI; + int collision_check_no_poses = -1; + std::string odom_topic = "odom"; + double controller_frequency = 10; } _params; diff --git a/mpc_local_planner/src/controller.cpp b/mpc_local_planner/src/controller.cpp index 3deec63..fe78489 100644 --- a/mpc_local_planner/src/controller.cpp +++ b/mpc_local_planner/src/controller.cpp @@ -856,4 +856,64 @@ bool Controller::generateInitialStateTrajectory(const Eigen::VectorXd& x0, const return true; } +bool Controller::isPoseTrajectoryFeasible(base_local_planner::CostmapModel* costmap_model, const std::vector& footprint_spec, + double inscribed_radius, double circumscribed_radius, double min_resolution_collision_check_angular, + int look_ahead_idx) +{ + if (!_grid) + { + ROS_ERROR("Controller must be configured before invoking step()."); + return false; + } + if (_grid->getN() < 2) return false; + + // we currently require a full discretization grid as we want to have fast access to + // individual states without requiring any new simulation. + // Alternatively, other grids could be used in combination with method getStateAndControlTimeSeries() + const FullDiscretizationGridBaseSE2* fd_grid = dynamic_cast(_grid.get()); + if (!fd_grid) + { + ROS_ERROR("isPoseTrajectoriyFeasible is currently only implemented for fd grids"); + return true; + } + + if (look_ahead_idx < 0 || look_ahead_idx >= _grid->getN()) look_ahead_idx = _grid->getN() - 1; + + for (int i = 0; i <= look_ahead_idx; ++i) + { + if (costmap_model->footprintCost(fd_grid->getState(i)[0], fd_grid->getState(i)[1], fd_grid->getState(i)[2], footprint_spec, inscribed_radius, + circumscribed_radius) == -1) + { + return false; + } + // Checks if the distance between two poses is higher than the robot radius or the orientation diff is bigger than the specified threshold + // and interpolates in that case. + // (if obstacles are pushing two consecutive poses away, the center between two consecutive poses might coincide with the obstacle ;-)! + if (i < look_ahead_idx) + { + double delta_rot = normalize_theta(fd_grid->getState(i)[i + 1] - fd_grid->getState(i)[0]); + Eigen::Vector2d delta_dist = fd_grid->getState(i + 1).head(2) - fd_grid->getState(i).head(2); + if (std::abs(delta_rot) > min_resolution_collision_check_angular || delta_dist.norm() > inscribed_radius) + { + int n_additional_samples = std::max(std::ceil(std::abs(delta_rot) / min_resolution_collision_check_angular), + std::ceil(delta_dist.norm() / inscribed_radius)) - + 1; + + PoseSE2 intermediate_pose(fd_grid->getState(i)[0], fd_grid->getState(i)[1], fd_grid->getState(i)[2]); + for (int step = 0; step < n_additional_samples; ++step) + { + intermediate_pose.position() = intermediate_pose.position() + delta_dist / (n_additional_samples + 1.0); + intermediate_pose.theta() = g2o::normalize_theta(intermediate_pose.theta() + delta_rot / (n_additional_samples + 1.0)); + if (costmap_model->footprintCost(intermediate_pose.x(), intermediate_pose.y(), intermediate_pose.theta(), footprint_spec, + inscribed_radius, circumscribed_radius) == -1) + { + return false; + } + } + } + } + } + return true; +} + } // namespace mpc_local_planner diff --git a/mpc_local_planner/src/mpc_local_planner_ros.cpp b/mpc_local_planner/src/mpc_local_planner_ros.cpp index 0dc5aa9..a923fcd 100644 --- a/mpc_local_planner/src/mpc_local_planner_ros.cpp +++ b/mpc_local_planner/src/mpc_local_planner_ros.cpp @@ -88,6 +88,10 @@ void MpcLocalPlannerROS::initialize(std::string name, tf2_ros::Buffer* tf, costm nh.param("collision_avoidance/costmap_obstacles_behind_robot_dist", _params.costmap_obstacles_behind_robot_dist, _params.costmap_obstacles_behind_robot_dist); + nh.param("collision_avoidance/collision_check_no_poses", _params.collision_check_no_poses, _params.collision_check_no_poses); + nh.param("collision_avoidance/collision_check_min_resolution_angular", _params.collision_check_min_resolution_angular, + _params.collision_check_min_resolution_angular); + // costmap converter plugin related parameters nh.param("costmap_converter_plugin", _costmap_conv_params.costmap_converter_plugin, _costmap_conv_params.costmap_converter_plugin); nh.param("costmap_converter_rate", _costmap_conv_params.costmap_converter_rate, _costmap_conv_params.costmap_converter_rate); @@ -371,25 +375,22 @@ uint32_t MpcLocalPlannerROS::computeVelocityCommands(const geometry_msgs::PoseSt costmap_2d::calculateMinAndMaxDistances(_footprint_spec, _robot_inscribed_radius, _robot_circumscribed_radius); } - /* - // check feasibility w.r.t. the original costmap footprint - bool feasible = planner_->isTrajectoryFeasible(costmap_model_.get(), footprint_spec_, robot_inscribed_radius_, robot_circumscribed_radius, - cfg_.trajectory.feasibility_check_no_poses); + bool feasible = _controller.isPoseTrajectoryFeasible(_costmap_model.get(), _footprint_spec, _robot_inscribed_radius, _robot_circumscribed_radius, + _params.collision_check_min_resolution_angular, _params.collision_check_no_poses); if (!feasible) { cmd_vel.twist.linear.x = cmd_vel.twist.linear.y = cmd_vel.twist.angular.z = 0; // now we reset everything to start again with the initialization of new trajectories. - planner_->clearPlanner(); - ROS_WARN("TebLocalPlannerROS: trajectory is not feasible. Resetting planner..."); - - ++no_infeasible_plans_; // increase number of infeasible solutions in a row - time_last_infeasible_plan_ = ros::Time::now(); - last_cmd_ = cmd_vel.twist; - message = "teb_local_planner trajectory is not feasible"; + _controller.reset(); // force reinitialization for next time + ROS_WARN("MpcLocalPlannerROS: trajectory is not feasible. Resetting planner..."); + ++_no_infeasible_plans; // increase number of infeasible solutions in a row + _time_last_infeasible_plan = ros::Time::now(); + _last_cmd = cmd_vel.twist; + message = "mpc_local_planner trajectory is not feasible"; return mbf_msgs::ExePathResult::NO_VALID_CMD; } -*/ + // Get the velocity command for this sampling interval // TODO(roesmann): we might also command more than just the imminent action, e.g. in a separate thread, until a new command is available if (!_u_seq || !_controller.getRobotDynamics()->getTwistFromControl(_u_seq->getValuesMap(0), cmd_vel.twist)) @@ -850,7 +851,8 @@ void MpcLocalPlannerROS::customViaPointsCB(const nav_msgs::Path::ConstPtr& via_p _custom_via_points_active = !_via_points.empty(); } -teb_local_planner::RobotFootprintModelPtr MpcLocalPlannerROS::getRobotFootprintFromParamServer(const ros::NodeHandle& nh, costmap_2d::Costmap2DROS* costmap_ros) +teb_local_planner::RobotFootprintModelPtr MpcLocalPlannerROS::getRobotFootprintFromParamServer(const ros::NodeHandle& nh, + costmap_2d::Costmap2DROS* costmap_ros) { std::string model_name; if (!nh.getParam("footprint_model/type", model_name)) diff --git a/mpc_local_planner_examples/cfg/carlike/mpc_local_planner_params.yaml b/mpc_local_planner_examples/cfg/carlike/mpc_local_planner_params.yaml index a0469db..b6bf30c 100644 --- a/mpc_local_planner_examples/cfg/carlike/mpc_local_planner_params.yaml +++ b/mpc_local_planner_examples/cfg/carlike/mpc_local_planner_params.yaml @@ -37,6 +37,8 @@ MpcLocalPlannerROS: cutoff_dist: 2.5 include_costmap_obstacles: True costmap_obstacles_behind_robot_dist: 1.0 + collision_check_no_poses: 5 + ## Planning grid grid: diff --git a/mpc_local_planner_examples/cfg/diff_drive/mpc_local_planner_params_quadratic_form.yaml b/mpc_local_planner_examples/cfg/diff_drive/mpc_local_planner_params_quadratic_form.yaml index 9445f8a..0360c7c 100644 --- a/mpc_local_planner_examples/cfg/diff_drive/mpc_local_planner_params_quadratic_form.yaml +++ b/mpc_local_planner_examples/cfg/diff_drive/mpc_local_planner_params_quadratic_form.yaml @@ -26,6 +26,7 @@ MpcLocalPlannerROS: cutoff_dist: 2.5 include_costmap_obstacles: True costmap_obstacles_behind_robot_dist: 1.5 + collision_check_no_poses: 5 ## Planning grid grid: