Added feasibility check with costmap robot model

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:

mpc_local_planner/include/mpc_local_planner/controller.h

@@ -33,6 +33,8 @@
 
 #include <mpc_local_planner_msgs/StateFeedback.h>
 
+#include <base_local_planner/costmap_model.h>
+
 #include <ros/subscriber.h>
 #include <ros/time.h>
 
@@ -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<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);
+
     // implements interface method
     void reset() override;
 

mpc_local_planner/include/mpc_local_planner/mpc_local_planner_ros.h

@@ -416,6 +416,8 @@ class MpcLocalPlannerROS : public nav_core::BaseLocalPlanner, public mbf_costmap
         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;
 

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<geometry_msgs::Point>& 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<const FullDiscretizationGridBaseSE2*>(_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

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))

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:

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: