适用场景
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吧?)
cppbase_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
cpp
#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 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();
};
};
#endifftc_planner.cpp
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 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();
}
}
}