张店制作网站seo快速排名站外流量推广
适用场景
1. 机器人要尽可能走直线
2. 遇到障碍物需要机器人停住, 不去尝试绕开障碍物
效果
- 机器人在收到目标点后, global_planner先生成一条直达该点的路径
- 机器人转向目标点
- 机器人移动至目标点
- 机器人旋转到目标位姿
全局路径规划
具体可以参考上篇文章, 修改了ROS自带navigation包中的carrot_planner, 使之具有以下特点
- global_plan这个vector中包含的路径点的数量增加, 适配局部路径规划
- 为global_plan中各个路径点添加位姿
- 可以将global_plan生成的路径以posearray的形式发布出来, 可以在rviz中监测
图中的红色密集箭头即为global_plan生成的全局路径, 话题为 /move_base_node/SimplePlanner/global_path
局部路径规划
参照ftc_local_planner进行修改, 特点如下
-
在重复修改终点时, 机器人不会有明显的减速, 停止, 再加速的过程
-
添加在旋转前的障碍物判定, 模拟自身旋转一周, 如果有碰撞风险则阻止此次旋转(参考…忘了抄的那里的了, 应该是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 degreesconst 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 <ros/ros.h>
#include "ftc_local_planner/PlannerGetProgress.h"
#include "ftc_local_planner/recovery_behaviors.h"#include <nav_msgs/Odometry.h>
#include <nav_msgs/Path.h>
#include <std_msgs/Bool.h>
#include <costmap_2d/costmap_2d_ros.h>
#include <tf/transform_listener.h>
#include <dynamic_reconfigure/server.h>
#include <ftc_local_planner/FTCPlannerROSConfig.h>
#include <ftc_local_planner/PID.h>
#include <nav_core/base_local_planner.h>
#include <tf2_ros/transform_broadcaster.h>
#include <tf2/LinearMath/Quaternion.h>
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
#include <nav_msgs/Odometry.h>
#include <tf2_ros/transform_listener.h>
#include <Eigen/Geometry>
#include "tf2_eigen/tf2_eigen.h"
#include <visualization_msgs/Marker.h>
#include <base_local_planner/world_model.h>
#include <base_local_planner/costmap_model.h>
#include <base_local_planner/odometry_helper_ros.h>
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 trackingPlannerState 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 usestd::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 stuckedros::Time time_last_oscillation_; //!< Store at which time stamp the last oscillation was detectedbool 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 <ftc_local_planner/ftc_planner.h>
#include <pluginlib/class_list_macros.h>PLUGINLIB_EXPORT_CLASS(ftc_local_planner::FTCPlannerROS, nav_core::BaseLocalPlanner)#define RET_SUCCESS 0
#define RET_COLLISION 104
#define RET_BLOCKED 109namespace 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 reconfigurereconfig_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 topicif (config.debug_pid){pubPid = private_nh.advertise<ftc_local_planner::PID>("debug_pid", 1, true);}// Recovery behavior initializationfailure_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 surecurrent_movement_speed = config.speed_slow;// set recovery behaviorfailure_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 aheadif(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 pointglobal_plan.push_back(global_plan.back());// give second from last point last oriantation as the point before thatglobal_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 distanceEigen::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 shortdouble 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 crashif (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 followingif (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 distancedouble 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);elsetf2::fromMsg(global_plan[1].pose, current_control_point);}break;case FOLLOWING:{// Normal planner operationdouble 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){// acceleratecurrent_movement_speed += dt * config.acceleration;if (current_movement_speed > speed)current_movement_speed = speed;}else if (speed < current_movement_speed){// deceleratecurrent_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 percentagedouble 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 pointsEigen::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 doneif (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 speedif (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 speedif (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 rotatingif (!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 rotatingif (!isRotateLegal()){ang_speed = 0;is_crashed = true;ROS_ERROR_STREAM("STOP ROTATE!!! Obstacle near by!");}cmd_vel.angular.z = ang_speed;// check if robot oscillatesbool 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;// proportionaldebugPidMsg.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;// integraldebugPidMsg.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;// diffdebugPidMsg.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;// errorsdebugPidMsg.lon_err = lon_error;debugPidMsg.lat_err = lat_error;debugPidMsg.ang_err = angle_error;// speedsdebugPidMsg.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 costsunsigned char previous_cost = 255;// ensure look ahead not out of planif (global_plan.size() < max_points){max_points = global_plan.size();}// calculate cost of footprint at robots actual poseif (config.obstacle_footprint){costmap->getOrientedFootprint(footprint);for (int i = 0; i < footprint.size(); i++){// check cost of each point of footprintif (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 obstacelif (costs > 0){// Possible collisionif (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 degreesconst 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 oscillationsif (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 detectedoscillation_detected_ = true;}// calculate duration of actual oscillationbool oscillation_duration_timeout = !((ros::Time::now() - time_last_oscillation_).toSec() < config.oscillation_recovery_min_duration); // check how long we oscillateif (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 oscillatingtime_last_oscillation_ = ros::Time::now(); // save time when oscillation was detectedoscillation_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();}}
}