一套简单的机器人短途路径规划算法

适用场景

1. 机器人要尽可能走直线
2. 遇到障碍物需要机器人停住, 不去尝试绕开障碍物

效果

1. 机器人在收到目标点后, global_planner先生成一条直达该点的路径
2. 机器人转向目标点
3. 机器人移动至目标点
4. 机器人旋转到目标位姿

全局路径规划

具体可以参考上篇文章, 修改了ROS自带navigation包中的carrot_planner, 使之具有以下特点

1. global_plan这个vector中包含的路径点的数量增加, 适配局部路径规划
2. 为global_plan中各个路径点添加位姿
3. 可以将global_plan生成的路径以posearray的形式发布出来, 可以在rviz中监测

图中的红色密集箭头即为global_plan生成的全局路径, 话题为 /move_base_node/SimplePlanner/global_path

局部路径规划

参照ftc_local_planner进行修改, 特点如下

1. 在重复修改终点时, 机器人不会有明显的减速, 停止, 再加速的过程

2. 添加在旋转前的障碍物判定, 模拟自身旋转一周, 如果有碰撞风险则阻止此次旋转(参考…忘了抄的那里的了, 应该是dwa吧?)

       base_local_planner::WorldModel* world_model_; ///< @brief The world model that the controller will use    
   	double FTCPlannerROS::footprintCost(double x_i, double y_i, double theta_i)
       {
           std::vector<geometry_msgs::Point> footprint = costmap->getRobotFootprint();
           double footprint_cost = world_model_->footprintCost(x_i, y_i, theta_i, footprint);
           return footprint_cost;
       }
     	//模拟旋转一周, 判断自身是否有几率碰撞到周围的障碍物
       bool FTCPlannerROS::isRotateLegal()
       {
           // ROS_ERROR("isRotateLegal is calling!!!");
           const double full_rotation_range = 2 * M_PI; // 360 degrees
           const double angle_increment = 0.01;
           // 这里的currenet_control_point是ftc在运行过程中生成的控制点, 本质是在global_plan中提取出来的一个位姿
           double currentX = current_control_point.translation().x();
           double currentY = current_control_point.translation().y();
           double currentYaw = current_control_point.rotation().eulerAngles(0, 1, 2).z();
           
           for (double angle = 0; angle < full_rotation_range; angle += angle_increment) 
           {
               double rotate_footprint_cost = footprintCost(currentX, currentY,(currentYaw + angle));
               if(rotate_footprint_cost >= costmap_2d::LETHAL_OBSTACLE)
               {
                   return false;
               } 
           }
           return true;
       }
   

完整代码

ftc_planner.hpp

#ifndef FTC_LOCAL_PLANNER_FTC_PLANNER_H_
#define FTC_LOCAL_PLANNER_FTC_PLANNER_H_

#include 
#include "ftc_local_planner/PlannerGetProgress.h"
#include "ftc_local_planner/recovery_behaviors.h"

#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include "tf2_eigen/tf2_eigen.h"
#include 
#include 
#include 
#include 
namespace ftc_local_planner
{

    class FTCPlannerROS : public nav_core::BaseLocalPlanner
    {

        enum PlannerState
        {
            PRE_ROTATE,
            FOLLOWING,
            WAITING_FOR_GOAL_APPROACH,
            POST_ROTATE,
            FINISHED
        };

    private:
        ros::ServiceServer progress_server;
        // State tracking
        PlannerState current_state;
        ros::Time state_entered_time;

        bool is_crashed;

        dynamic_reconfigure::Server<FTCPlannerROSConfig> *reconfig_server;

        tf2_ros::Buffer *tf_buffer;
        costmap_2d::Costmap2DROS *costmap;
        costmap_2d::Costmap2D* costmap_map_;   
        base_local_planner::WorldModel* world_model_; ///< @brief The world model that the controller will use
        std::vector<geometry_msgs::PoseStamped> global_plan;
        ros::Publisher global_point_pub;
        ros::Publisher global_plan_pub;
        ros::Publisher progress_pub;
        ros::Publisher obstacle_marker_pub;
        ros::Publisher obstacle_observer_pub;
        
        FTCPlannerROSConfig config;

        Eigen::Affine3d current_control_point;

        /**
         * PID State
         */
        double lat_error, lon_error, angle_error = 0.0;
        double last_lon_error = 0.0;
        double last_lat_error = 0.0;
        double last_angle_error = 0.0;
        double i_lon_error = 0.0;
        double i_lat_error = 0.0;
        double i_angle_error = 0.0;
        ros::Time last_time;

        /**
         * Speed ramp for acceleration and deceleration
         */
        double current_movement_speed;

        /**
         * State for point interpolation
         */
        uint32_t current_index;
        uint32_t obstacle_index;
        double current_progress;
        Eigen::Affine3d local_control_point;

        /**
         * Private members
         */
        ros::Publisher pubPid;
        FailureDetector failure_detector_; //!< Detect if the robot got stucked
        ros::Time time_last_oscillation_;  //!< Store at which time stamp the last oscillation was detected
        bool oscillation_detected_ = false;
        bool oscillation_warning_ = false;

        /**
         * odom_helper 
         */        
        base_local_planner::OdometryHelperRos odom_helper_;
        std::string odom_topic_;

        double distanceLookahead();
        PlannerState update_planner_state();
        void update_control_point(double dt);
        void calculate_velocity_commands(double dt, geometry_msgs::Twist &cmd_vel);

        /**
         * @brief check for obstacles in path as well as collision at actual pose
         * @param max_points number of path segments (of global path) to check
         * @return true if collision will happen.
         */
        bool checkCollision(int max_points);
        double footprintCost(double x_i, double y_i, double theta_i);
        bool isRotateLegal();

        /**
         * @brief check if robot oscillates (only angular). Can be used to do some recovery
         * @param cmd_vel last velocity message send to robot
         * @return true if robot oscillates
         */
        bool checkOscillation(geometry_msgs::Twist &cmd_vel);

        /**
         * @brief publish obstacles on path as marker array.
         * @brief If obstacle_points contains more elements than maxID, marker gets published and
         * @brief cleared afterwards.
         * @param obstacle_points already collected points to visualize
         * @param x X position in costmap
         * @param y Y position in costmap
         * @param cost cost value of cell
         * @param maxIDs num of markers before publishing
         * @return sum of `values`, or 0.0 if `values` is empty.
         */
        void debugObstacle(visualization_msgs::Marker &obstacle_points, double x, double y, unsigned char cost, int maxIDs);

        double time_in_current_state()
        {
            return (ros::Time::now() - state_entered_time).toSec();
        }

        void reconfigureCB(FTCPlannerROSConfig &config, uint32_t level);

    public:
        FTCPlannerROS();

        ~FTCPlannerROS();

        bool getProgress(ftc_local_planner::PlannerGetProgressRequest &req, ftc_local_planner::PlannerGetProgressResponse &res);

        bool setPlan(const std::vector<geometry_msgs::PoseStamped> &plan);

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

        bool computeVelocityCommands(geometry_msgs::Twist& cmd_vel);

        bool isGoalReached();

        bool cancel();
    };
};
#endif

ftc_planner.cpp

#include 
#include 

PLUGINLIB_EXPORT_CLASS(ftc_local_planner::FTCPlannerROS, nav_core::BaseLocalPlanner)

#define RET_SUCCESS 0
#define RET_COLLISION 104
#define RET_BLOCKED 109

namespace ftc_local_planner
{

    FTCPlannerROS::FTCPlannerROS():odom_helper_("odom")
    {
    }

    void FTCPlannerROS::initialize(std::string name, tf2_ros::Buffer *tf, costmap_2d::Costmap2DROS *costmap_ros)
    {
        ros::NodeHandle private_nh("~/" + name);

        progress_server = private_nh.advertiseService(
            "planner_get_progress", &FTCPlannerROS::getProgress, this);

        global_point_pub = private_nh.advertise<geometry_msgs::PoseStamped>("global_point", 1);
        global_plan_pub = private_nh.advertise<nav_msgs::Path>("global_plan", 1, true);
        obstacle_marker_pub = private_nh.advertise<visualization_msgs::Marker>("costmap_marker", 10);
        obstacle_observer_pub = private_nh.advertise<std_msgs::Bool>("obstacle_detection", 10);

        costmap = costmap_ros;
        costmap_map_ = costmap->getCostmap();
        world_model_ = new base_local_planner::CostmapModel(*costmap_map_); 
        tf_buffer = tf;

        // Parameter for dynamic reconfigure
        reconfig_server = new dynamic_reconfigure::Server<FTCPlannerROSConfig>(private_nh);
        dynamic_reconfigure::Server<FTCPlannerROSConfig>::CallbackType cb = boost::bind(&FTCPlannerROS::reconfigureCB, this,
                                                                                     _1, _2);
        reconfig_server->setCallback(cb);

        current_state = PRE_ROTATE;

        // PID Debugging topic
        if (config.debug_pid)
        {
            pubPid = private_nh.advertise<ftc_local_planner::PID>("debug_pid", 1, true);
        }

        // Recovery behavior initialization
        failure_detector_.setBufferLength(std::round(config.oscillation_recovery_min_duration * 10));

        ROS_INFO("FTCLocalPlannerROS: Version 2 Init.");
    }

    void FTCPlannerROS::reconfigureCB(FTCPlannerROSConfig &c, uint32_t level)
    {
        if (c.restore_defaults)
        {
            reconfig_server->getConfigDefault(c);
            c.restore_defaults = false;
        }
        config = c;

        // just to be sure
        current_movement_speed = config.speed_slow;

        // set recovery behavior
        failure_detector_.setBufferLength(std::round(config.oscillation_recovery_min_duration * 10));
    }

    bool FTCPlannerROS::setPlan(const std::vector<geometry_msgs::PoseStamped> &plan)
    {
        current_state = PRE_ROTATE;
        state_entered_time = ros::Time::now();
        is_crashed = false;

        global_plan = plan;

        // if whole_distance is less than 0.1m then follow in detail, 
        // else set control_point further to avoid discrete speed control.
        double whole_distance, diffX, diffY;
        diffX = global_plan[0].pose.position.x - global_plan[global_plan.size()-1].pose.position.x;
        diffY = global_plan[0].pose.position.y - global_plan[global_plan.size()-1].pose.position.y;
        whole_distance = std::sqrt(diffX * diffX + diffY * diffY);
        // if length of whole path is over 1 [m], set current_index ahead
        if(whole_distance >=1) current_index = 18;
        else current_index = 1;
        obstacle_index = 0;
        current_progress = 0.0;

        last_time = ros::Time::now();
        current_movement_speed = config.speed_slow;

        lat_error = 0.0;
        lon_error = 0.0;
        angle_error = 0.0;
        i_lon_error = 0.0;
        i_lat_error = 0.0;
        i_angle_error = 0.0;

        nav_msgs::Path path;

        if (global_plan.size() > 2)
        {
            // duplicate last point
            global_plan.push_back(global_plan.back());
            // give second from last point last oriantation as the point before that
            global_plan[global_plan.size() - 2].pose.orientation = global_plan[global_plan.size() - 3].pose.orientation;
            path.header = plan.front().header;
            path.poses = plan;
        }
        else
        {
            ROS_WARN_STREAM("FTCLocalPlannerROS: Global plan was too short. Need a minimum of 3 poses - Cancelling.");
            current_state = FINISHED;
            state_entered_time = ros::Time::now();
        }
        global_plan_pub.publish(path);

        ROS_INFO_STREAM("FTCLocalPlannerROS: Got new global plan with " << plan.size() << " points.");

        return true;
    }

    FTCPlannerROS::~FTCPlannerROS()
    {
        if (reconfig_server != nullptr)
        {
            delete reconfig_server;
            reconfig_server = nullptr;
        }
        delete world_model_;
    }

    double FTCPlannerROS::distanceLookahead()
    {
        if (global_plan.size() < 2)
        {
            return 0;
        }
        Eigen::Quaternion<double> current_rot(current_control_point.linear());

        Eigen::Affine3d last_straight_point = current_control_point;
        for (uint32_t i = current_index + 1; i < global_plan.size(); i++)
        {
            tf2::fromMsg(global_plan[i].pose, last_straight_point);
            // check, if direction is the same. if so, we add the distance
            Eigen::Quaternion<double> rot2(last_straight_point.linear());
            if (abs(rot2.angularDistance(current_rot)) > config.speed_fast_threshold_angle * (M_PI / 180.0))
            {
                break;
            }
        }

        return (last_straight_point.translation() - current_control_point.translation()).norm();
    }

    bool FTCPlannerROS::computeVelocityCommands(geometry_msgs::Twist &cmd_vel)
    {

        ros::Time now = ros::Time::now();
        double dt = now.toSec() - last_time.toSec();
        last_time = now;

        if (is_crashed)
        {
            cmd_vel.linear.x = 0;
            cmd_vel.angular.z = 0;
            return false;
        }

        if (current_state == FINISHED)
        {
            cmd_vel.linear.x = 0;
            cmd_vel.angular.z = 0;
            return true;
        }

        // We're not crashed and not finished.
        // First, we update the control point if needed. This is needed since we need the local_control_point to calculate the next state.
        update_control_point(dt);
        // Then, update the planner state.
        auto new_planner_state = update_planner_state();
        if (new_planner_state != current_state)
        {
            ROS_INFO_STREAM("FTCLocalPlannerROS: Switching to state " << new_planner_state);
            state_entered_time = ros::Time::now();
            current_state = new_planner_state;
        }

        if (checkCollision(config.obstacle_lookahead))
        {
            cmd_vel.linear.x = 0;
            cmd_vel.angular.z = 0;
            is_crashed = true;
            return false;
        }

        // Finally, we calculate the velocity commands.
        calculate_velocity_commands(dt, cmd_vel);

        if (is_crashed)
        {
            cmd_vel.linear.x = 0;
            cmd_vel.angular.z = 0;
            return false;
        }

        return true;
    }

    bool FTCPlannerROS::isGoalReached()
    {
        return current_state == FINISHED && !is_crashed;
    }

    bool FTCPlannerROS::cancel()
    {
        ROS_WARN_STREAM("FTCLocalPlannerROS: FTC planner was cancelled.");
        current_state = FINISHED;
        state_entered_time = ros::Time::now();
        return true;
    }

    FTCPlannerROS::PlannerState FTCPlannerROS::update_planner_state()
    {
        switch (current_state)
        {
        case PRE_ROTATE:
        {
            if (time_in_current_state() > config.goal_timeout)
            {
                ROS_ERROR_STREAM("FTCLocalPlannerROS: Error reaching goal. config.goal_timeout (" << config.goal_timeout << ") reached - Timeout in PRE_ROTATE phase.");
                is_crashed = true;
                return FINISHED;
            }
            // calculate total distance, STOP if whole path is too short
            double whole_distance, diffX, diffY;
            diffX = global_plan[0].pose.position.x - global_plan[global_plan.size()-1].pose.position.x;
            diffY = global_plan[0].pose.position.y - global_plan[global_plan.size()-1].pose.position.y;
            whole_distance = std::sqrt(diffX * diffX + diffY * diffY);
            if (whole_distance <= 0.08) 
            {
                ROS_ERROR("FTC plan is too short, abort!");
                return FINISHED;
            }
            if (abs(angle_error) * (180.0 / M_PI) < config.max_goal_angle_error || !isRotateLegal())
            {
                std::cout<<"current angle error: "<<angle_error<<std::endl;
                std::cout<<"current angle error in degree: "<<abs(angle_error) * (180.0 / M_PI)<<std::endl;
                std::cout<<"config.max_goal_angle_error: " << config.max_goal_angle_error<<std::endl;
                ROS_INFO_STREAM("FTCLocalPlannerROS: PRE_ROTATE finished. Starting following");
                ROS_INFO("Switching state to FOLLOWING!!!");
                return FOLLOWING;
            }
        }
        break;
        case FOLLOWING:
        {
            double distance = local_control_point.translation().norm();
            // check for crash
            if (distance > config.max_follow_distance)
            {
                ROS_ERROR_STREAM("FTCLocalPlannerROS: Robot is far away from global plan. distance (" << distance << ") > config.max_follow_distance (" << config.max_follow_distance << ") It probably has crashed.");
                is_crashed = true;
                return FINISHED;
            }

            // check if we're done following
            if (current_index == global_plan.size() - 2)
            {
                ROS_INFO_STREAM("FTCLocalPlannerROS: switching planner to position mode");
                ROS_INFO("FOLLOWING finished!");
                ROS_INFO("Switching to WAITING_FOR_GOAL_APPROACH");
                return WAITING_FOR_GOAL_APPROACH;
            }
        }
        break;
        case WAITING_FOR_GOAL_APPROACH:
        {
            double distance = local_control_point.translation().norm();
            if (time_in_current_state() > config.goal_timeout)
            {
                ROS_WARN_STREAM("FTCLocalPlannerROS: Could not reach goal position. config.goal_timeout (" << config.goal_timeout << ") reached - Attempting final rotation anyways.");
                return POST_ROTATE;
            }
            if (distance < config.max_goal_distance_error)
            {
                ROS_INFO("WAITING_FOR_GOAL_APPROACH finished!");
                ROS_INFO("Switching to  POST_ROTATE!");
                ROS_INFO_STREAM("FTCLocalPlannerROS: Reached goal position.");
                return POST_ROTATE;
            }
        }
        break;
        case POST_ROTATE:
        {
            if (time_in_current_state() > config.goal_timeout)
            {
                ROS_WARN_STREAM("FTCLocalPlannerROS: Could not reach goal rotation. config.goal_timeout (" << config.goal_timeout << ") reached");
                return FINISHED;
            }
            // calculate total distance
            double total_distance=0.0;
            double diffX;
            double diffY;
            if (current_control_point.translation().x())
            {
                diffX = current_control_point.translation().x() - global_plan[global_plan.size()-1].pose.position.x;
                diffY = current_control_point.translation().y() - global_plan[global_plan.size()-1].pose.position.y;
            }
            else
            {
                diffX = global_plan[0].pose.position.x - global_plan[global_plan.size()-1].pose.position.x;
                diffY = global_plan[0].pose.position.y - global_plan[global_plan.size()-1].pose.position.y;
            }
            total_distance = std::sqrt(diffX * diffX + diffY * diffY);
            if (total_distance <= 0.08) 
            {
                ROS_ERROR("FTC plan is too short, abort!");
                return FINISHED;
            }
            if (abs(angle_error) * (180.0 / M_PI) < config.max_goal_angle_error || !isRotateLegal())
            {
                ROS_INFO_STREAM("FTCLocalPlannerROS: POST_ROTATE finished.");
                return FINISHED;
            }
        }
        break;
        case FINISHED:
        {
            ROS_INFO("Move finished!");
        }
        break;
        }

        return current_state;
    }

    void FTCPlannerROS::update_control_point(double dt)
    {

        switch (current_state)
        {
        case PRE_ROTATE:
        {
            nav_msgs::Odometry current_odom;
            odom_helper_.getOdom(current_odom);
            // if current speed is over 0.02 [m/s] set control_point ahead.
            if(abs(current_odom.twist.twist.linear.x)>=0.02) 
                tf2::fromMsg(global_plan[current_index].pose, current_control_point);
            else
                tf2::fromMsg(global_plan[1].pose, current_control_point);
        }
        break;
        case FOLLOWING:
        {
            // Normal planner operation
            double straight_dist = distanceLookahead();
            double speed;
            if (straight_dist >= config.speed_fast_threshold)
            {
                speed = config.speed_fast;
            }
            else
            {
                speed = config.speed_slow;
            }

            if (speed > current_movement_speed)
            {
                // accelerate
                current_movement_speed += dt * config.acceleration;
                if (current_movement_speed > speed)
                    current_movement_speed = speed;
            }
            else if (speed < current_movement_speed)
            {
                // decelerate
                current_movement_speed -= dt * config.acceleration;
                if (current_movement_speed < speed)
                    current_movement_speed = speed;
            }

            double distance_to_move = dt * current_movement_speed;
            double angle_to_move = dt * config.speed_angular * (M_PI / 180.0);

            Eigen::Affine3d nextPose, currentPose;
            while (angle_to_move > 0 && distance_to_move > 0 && current_index < global_plan.size() - 2)
            {

                tf2::fromMsg(global_plan[current_index].pose, currentPose);
                tf2::fromMsg(global_plan[current_index + 1].pose, nextPose);

                double pose_distance = (nextPose.translation() - currentPose.translation()).norm();

                Eigen::Quaternion<double> current_rot(currentPose.linear());
                Eigen::Quaternion<double> next_rot(nextPose.linear());

                double pose_distance_angular = current_rot.angularDistance(next_rot);

                if (pose_distance <= 0.0)
                {
                    ROS_WARN_STREAM("FTCLocalPlannerROS: Skipping duplicate point in global plan.");
                    current_index++;
                    obstacle_index++;
                    continue;
                }

                double remaining_distance_to_next_pose = pose_distance * (1.0 - current_progress);
                double remaining_angular_distance_to_next_pose = pose_distance_angular * (1.0 - current_progress);

                if (remaining_distance_to_next_pose < distance_to_move &&
                    remaining_angular_distance_to_next_pose < angle_to_move)
                {
                    // we need to move further than the remaining distance_to_move. Skip to the next point and decrease distance_to_move.
                    current_progress = 0.0;
                    current_index++;
                    obstacle_index++;
                    distance_to_move -= remaining_distance_to_next_pose;
                    angle_to_move -= remaining_angular_distance_to_next_pose;
                }
                else
                {
                    // we cannot reach the next point yet, so we update the percentage
                    double current_progress_distance =
                        (pose_distance * current_progress + distance_to_move) / pose_distance;
                    double current_progress_angle =
                        (pose_distance_angular * current_progress + angle_to_move) / pose_distance_angular;
                    current_progress = fmin(current_progress_angle, current_progress_distance);
                    if (current_progress > 1.0)
                    {
                        ROS_WARN_STREAM("FTCLocalPlannerROS: FTC PLANNER: Progress > 1.0");
                        //                    current_progress = 1.0;
                    }
                    distance_to_move = 0;
                    angle_to_move = 0;
                }
            }

            tf2::fromMsg(global_plan[current_index].pose, currentPose);
            tf2::fromMsg(global_plan[current_index + 1].pose, nextPose);
            // interpolate between points
            Eigen::Quaternion<double> rot1(currentPose.linear());
            Eigen::Quaternion<double> rot2(nextPose.linear());

            Eigen::Vector3d trans1 = currentPose.translation();
            Eigen::Vector3d trans2 = nextPose.translation();

            Eigen::Affine3d result;
            result.translation() = (1.0 - current_progress) * trans1 + current_progress * trans2;
            result.linear() = rot1.slerp(current_progress, rot2).toRotationMatrix();

            current_control_point = result;
        }
        break;
        case POST_ROTATE:
            tf2::fromMsg(global_plan[global_plan.size() - 1].pose, current_control_point);
            break;
        case WAITING_FOR_GOAL_APPROACH:
            break;
        case FINISHED:
            break;
        }

        {
            geometry_msgs::PoseStamped viz;
            viz.header = global_plan[current_index].header;
            viz.pose = tf2::toMsg(current_control_point);
            global_point_pub.publish(viz);
        }
        auto map_to_base = tf_buffer->lookupTransform("base_link", "map", ros::Time(), ros::Duration(1.0));
        tf2::doTransform(current_control_point, local_control_point, map_to_base);

        lat_error = local_control_point.translation().y();
        lon_error = local_control_point.translation().x();
        angle_error = local_control_point.rotation().eulerAngles(0, 1, 2).z();
    }

    void FTCPlannerROS::calculate_velocity_commands(double dt, geometry_msgs::Twist &cmd_vel)
    {
        // check, if we're completely done
        if (current_state == FINISHED || is_crashed)
        {
            ROS_DEBUG_STREAM("current state==FINISHED!, set cme_vel = 0.0");
            cmd_vel.linear.x = 0;
            cmd_vel.angular.z = 0;
            return;
        }

        i_lon_error += lon_error * dt;
        i_lat_error += lat_error * dt;
        i_angle_error += angle_error * dt;

        if (i_lon_error > config.ki_lon_max)
        {
            i_lon_error = config.ki_lon_max;
        }
        else if (i_lon_error < -config.ki_lon_max)
        {
            i_lon_error = -config.ki_lon_max;
        }
        if (i_lat_error > config.ki_lat_max)
        {
            i_lat_error = config.ki_lat_max;
        }
        else if (i_lat_error < -config.ki_lat_max)
        {
            i_lat_error = -config.ki_lat_max;
        }
        if (i_angle_error > config.ki_ang_max)
        {
            i_angle_error = config.ki_ang_max;
        }
        else if (i_angle_error < -config.ki_ang_max)
        {
            i_angle_error = -config.ki_ang_max;
        }

        double d_lat = (lat_error - last_lat_error) / dt;
        double d_lon = (lon_error - last_lon_error) / dt;
        double d_angle = (angle_error - last_angle_error) / dt;

        last_lat_error = lat_error;
        last_lon_error = lon_error;
        last_angle_error = angle_error;

        // linear movement is only allowed in FOLLOWING state
        // calculate linear speed
        if (current_state == FOLLOWING)
        {
            double lin_speed = lon_error * config.kp_lon + i_lon_error * config.ki_lon + d_lon * config.kd_lon;
            if (lin_speed < 0 && config.forward_only)
            {
                lin_speed = 0;
            }
            else
            {
                if (lin_speed > config.max_cmd_vel_speed)
                {
                    lin_speed = config.max_cmd_vel_speed;
                }
                else if (lin_speed < -config.max_cmd_vel_speed)
                {
                    lin_speed = -config.max_cmd_vel_speed;
                }

                if (lin_speed < 0)
                {
                    lat_error *= -1.0;
                }
            }
            cmd_vel.linear.x = lin_speed;
        }
        else
        {
            cmd_vel.linear.x = 0.0;
        }

        // calculate angular speed
        if (current_state == FOLLOWING)
        {

            double ang_speed = angle_error * config.kp_ang + i_angle_error * config.ki_ang + d_angle * config.kd_ang +
                               lat_error * config.kp_lat + i_lat_error * config.ki_lat + d_lat * config.kd_lat;

            if (ang_speed > config.max_cmd_vel_ang)
            {
                ang_speed = config.max_cmd_vel_ang;
            }
            else if (ang_speed < -config.max_cmd_vel_ang)
            {
                ang_speed = -config.max_cmd_vel_ang;
            }
            // check whether obstacle exists while rotating
            if (!isRotateLegal())
            {
                ang_speed = 0;
                is_crashed = true;
                ROS_ERROR_STREAM("STOP ROTATE!!! Obstacle near by!");
            }
            cmd_vel.angular.z = ang_speed;
        }
        else
        {
            double ang_speed = angle_error * config.kp_ang + i_angle_error * config.ki_ang + d_angle * config.kd_ang;
            if (ang_speed > config.max_cmd_vel_ang)
            {
                ang_speed = config.max_cmd_vel_ang;
            }
            else if (ang_speed < -config.max_cmd_vel_ang)
            {
                ang_speed = -config.max_cmd_vel_ang;
            }
            // check whether obstacle exists while rotating
            if (!isRotateLegal())
            {
                ang_speed = 0;
                is_crashed = true;
                ROS_ERROR_STREAM("STOP ROTATE!!! Obstacle near by!");
            }
            cmd_vel.angular.z = ang_speed;

            // check if robot oscillates
            bool is_oscillating = checkOscillation(cmd_vel);
            if (is_oscillating)
            {
                ang_speed = config.max_cmd_vel_ang;
                cmd_vel.angular.z = ang_speed;
            }
        }

        if (config.debug_pid)
        {
            ftc_local_planner::PID debugPidMsg;
            debugPidMsg.kp_lon_set = lon_error;

            // proportional
            debugPidMsg.kp_lat_set = lat_error * config.kp_lat;
            debugPidMsg.kp_lon_set = lon_error * config.kp_lon;
            debugPidMsg.kp_ang_set = angle_error * config.kp_ang;

            // integral
            debugPidMsg.ki_lat_set = i_lat_error * config.ki_lat;
            debugPidMsg.ki_lon_set = i_lon_error * config.ki_lon;
            debugPidMsg.ki_ang_set = i_angle_error * config.ki_ang;

            // diff
            debugPidMsg.kd_lat_set = d_lat * config.kd_lat;
            debugPidMsg.kd_lon_set = d_lon * config.kd_lon;
            debugPidMsg.kd_ang_set = d_angle * config.kd_ang;

            // errors
            debugPidMsg.lon_err = lon_error;
            debugPidMsg.lat_err = lat_error;
            debugPidMsg.ang_err = angle_error;

            // speeds
            debugPidMsg.ang_speed = cmd_vel.angular.z;
            debugPidMsg.lin_speed = cmd_vel.linear.x;

            pubPid.publish(debugPidMsg);
        }
    }

    bool FTCPlannerROS::getProgress(PlannerGetProgressRequest &req, PlannerGetProgressResponse &res)
    {
        res.index = current_index;
        return true;
    }

    bool FTCPlannerROS::checkCollision(int max_points)
    {
        unsigned int x;
        unsigned int y;

        std::vector<geometry_msgs::Point> footprint;
        visualization_msgs::Marker obstacle_marker;

        if (!config.check_obstacles)
        {
            return false;
        }
        // maximal costs
        unsigned char previous_cost = 255;
        // ensure look ahead not out of plan
        if (global_plan.size() < max_points)
        {
            max_points = global_plan.size();
        }

        // calculate cost of footprint at robots actual pose
        if (config.obstacle_footprint)
        {
        costmap->getOrientedFootprint(footprint);
        for (int i = 0; i < footprint.size(); i++)
        {
            // check cost of each point of footprint
            if (costmap_map_->worldToMap(footprint[i].x, footprint[i].y, x, y))
            {
                unsigned char costs = costmap_map_->getCost(x, y);
                if (costs >= costmap_2d::LETHAL_OBSTACLE)
                {
                    ROS_WARN("FTCLocalPlannerROS: Possible collision of footprint at actual pose. Stop local planner.");
                    std_msgs::Bool obstacle_msg;
                    obstacle_msg.data = true;
                    obstacle_observer_pub.publish(obstacle_msg);
                    return true;
                }
            }
        }
        }

        for (int i = 0; i < max_points; i++)
        {
            geometry_msgs::PoseStamped x_pose;
            int index = obstacle_index + i;
            if (index > global_plan.size())
            {
                index = global_plan.size();
            }
            x_pose = global_plan[index];

            if (costmap_map_->worldToMap(x_pose.pose.position.x, x_pose.pose.position.y, x, y))
            {
                unsigned char costs = costmap_map_->getCost(x, y);
                if (config.debug_obstacle)
                {
                    debugObstacle(obstacle_marker, x, y, costs, max_points);
                }
                // Near at obstacel
                if (costs > 0)
                {
                    // Possible collision
                    if (costs > 127 && costs > previous_cost)
                    {
                        ROS_WARN("FTCLocalPlannerROS: Possible collision. Stop local planner.");
                        std_msgs::Bool obstacle_msg;
                        obstacle_msg.data = true;
                        obstacle_observer_pub.publish(obstacle_msg);   
                        return true;
                    }
                }
                previous_cost = costs;
            }
        }
        return false;
    }
    
    bool FTCPlannerROS::isRotateLegal()
    {
        // ROS_ERROR("isRotateLegal is calling!!!");
        const double full_rotation_range = 2 * M_PI; // 360 degrees
        const double angle_increment = 0.01;
        double currentX = current_control_point.translation().x();
        double currentY = current_control_point.translation().y();
        double currentYaw = current_control_point.rotation().eulerAngles(0, 1, 2).z();
        
        for (double angle = 0; angle < full_rotation_range; angle += angle_increment) 
        {
            double rotate_footprint_cost = footprintCost(currentX, currentY,(currentYaw + angle));
            if(rotate_footprint_cost >= costmap_2d::LETHAL_OBSTACLE)
            {
                ROS_DEBUG_STREAM("================================");
                ROS_DEBUG_STREAM("FTC: Rotate is illegal");
                ROS_DEBUG_STREAM("FTC: switch to next state!");
                ROS_DEBUG_STREAM("================================");
                return false;
            } 
        }
        return true;
    }

    double FTCPlannerROS::footprintCost(double x_i, double y_i, double theta_i)
    {
        std::vector<geometry_msgs::Point> footprint = costmap->getRobotFootprint();
        double footprint_cost = world_model_->footprintCost(x_i, y_i, theta_i, footprint);
        return footprint_cost;
    }

    bool FTCPlannerROS::checkOscillation(geometry_msgs::Twist &cmd_vel)
    {
        bool oscillating = false;
        // detect and resolve oscillations
        if (config.oscillation_recovery)
        {
            // oscillating = true;
            double max_vel_theta = config.max_cmd_vel_ang;
            double max_vel_current = config.max_cmd_vel_speed;

            failure_detector_.update(cmd_vel, config.max_cmd_vel_speed, config.max_cmd_vel_speed, max_vel_theta,
                                     config.oscillation_v_eps, config.oscillation_omega_eps);

            oscillating = failure_detector_.isOscillating();

            if (oscillating) // we are currently oscillating
            {
                if (!oscillation_detected_) // do we already know that robot oscillates?
                {
                    time_last_oscillation_ = ros::Time::now(); // save time when oscillation was detected
                    oscillation_detected_ = true;
                }
                // calculate duration of actual oscillation
                bool oscillation_duration_timeout = !((ros::Time::now() - time_last_oscillation_).toSec() < config.oscillation_recovery_min_duration); // check how long we oscillate
                if (oscillation_duration_timeout)
                {
                    if (!oscillation_warning_) // ensure to send warning just once instead of spamming around
                    {
                        ROS_WARN("FTCLocalPlannerROS: possible oscillation (of the robot or its local plan) detected. Activating recovery strategy (prefer current turning direction during optimization).");
                        oscillation_warning_ = true;
                    }
                    return true;
                }
                return false; // oscillating but timeout not reached
            }
            else
            {
                // not oscillating
                time_last_oscillation_ = ros::Time::now(); // save time when oscillation was detected
                oscillation_detected_ = false;
                oscillation_warning_ = false;
                return false;
            }
        }
        return false; // no check for oscillation
    }

    void FTCPlannerROS::debugObstacle(visualization_msgs::Marker &obstacle_points, double x, double y, unsigned char cost, int maxIDs)
    {
        if (obstacle_points.points.empty())
        {
            obstacle_points.header.frame_id = costmap->getGlobalFrameID();
            obstacle_points.header.stamp = ros::Time::now();
            obstacle_points.action = visualization_msgs::Marker::ADD;
            obstacle_points.pose.orientation.w = 1.0;
            obstacle_points.type = visualization_msgs::Marker::POINTS;
            obstacle_points.scale.x = 0.2;
            obstacle_points.scale.y = 0.2;
        }
        obstacle_points.id = obstacle_points.points.size() + 1;

        if (cost < 127)
        {
            obstacle_points.color.g = 1.0f;
        }

        if (cost >= 127 && cost < 255)
        {
            obstacle_points.color.r = 1.0f;
        }
        obstacle_points.color.a = 1.0;
        geometry_msgs::Point p;
        costmap_map_->mapToWorld(x, y, p.x, p.y);
        p.z = 0;

        obstacle_points.points.push_back(p);
        if (obstacle_points.points.size() >= maxIDs || cost > 0)
        {
            obstacle_marker_pub.publish(obstacle_points);
            obstacle_points.points.clear();
        }
    }
}