动态窗口法是一种比较常用的局部路径规划算法。ros的局部路径规划中也有这种算法。本文将对ros中集成的dwa局部路径规划算法进行解析。
所谓DWA局部路径算法其实就是在速度空间内根据一定的规则，产生多组的机器人运动速度。然后在设定的时间的进行模拟，产生相应的轨迹然后对产生的轨迹进行评价，根据自己设定的评价函数进行判断，得分最高的就是局部路径规划所要产生的速度。
该算法有两个较为核心的问题。一个是速度空间的确定，而是轨迹评价函数的确定。
关于dwa算法的理论下面的文章已经有了详尽的介绍，在此不再赘述。
https://blog.csdn.net/heyijia0327/article/details/44983551
下面将会将代码进行分析。dwa_planner.cpp:
#include <dwa_local_planner/dwa_planner.h>
#include <base_local_planner/goal_functions.h>
#include <base_local_planner/map_grid_cost_point.h>
#include <cmath>
//for computing path distance
#include <queue>
#include <angles/angles.h>
#include <ros/ros.h>
#include <pcl_conversions/pcl_conversions.h>
namespace dwa_local_planner {
void DWAPlanner::reconfigure(DWAPlannerConfig &config)
{
boost::mutex::scoped_lock l(configuration_mutex_);
generator_.setParameters(
config.sim_time,
config.sim_granularity,
config.angular_sim_granularity,
config.use_dwa,
sim_period_);
double resolution = planner_util_->getCostmap()->getResolution();
pdist_scale_ = config.path_distance_bias;
gdist_scale_ = config.goal_distance_bias;
occdist_scale_ = config.occdist_scale;
alignment_scale_ = config.alignment_scale;
goal_front_scale_ = config.goal_front_scale;
additional_goal_scale_ = config.additional_goal_scale;
forward_point_distance_ = config.forward_point_distance;
alignment_point_distance_ = config.alignment_point_distance;
additional_goal_distance_ = config.additional_goal_distance;
// pdistscale used for both path and alignment, set forward_point_distance to zero to discard alignment
path_costs_.setScale(resolution * pdist_scale_ * 0.5);
alignment_costs_.setScale(resolution * alignment_scale_ * 0.5);
goal_costs_.setScale(resolution * gdist_scale_ * 0.5);
goal_front_costs_.setScale(resolution * goal_front_scale_ * 0.5);
goal_front_costs_.setXShift(forward_point_distance_);
additional_goal_costs_.setScale(resolution * additional_goal_scale_ * 1.0);
alignment_costs_.setXShift(alignment_point_distance_);
obstacle_costs_.setScale(resolution * occdist_scale_);
stop_time_buffer_ = config.stop_time_buffer;
oscillation_costs_.setOscillationResetDist(config.oscillation_reset_dist, config.oscillation_reset_angle);
// obstacle costs can vary due to scaling footprint feature
obstacle_costs_.setParams(config.max_trans_vel, config.max_scaling_factor, config.scaling_speed);
int vx_samp, vy_samp, vth_samp;
vx_samp = config.vx_samples;
vy_samp = config.vy_samples;
vth_samp = config.vth_samples;
if (vx_samp <= 0) {
ROS_WARN("You've specified that you don't want any samples in the x dimension. We'll at least assume that you want to sample one value... so we're going to set vx_samples to 1 instead");
vx_samp = 1;
config.vx_samples = vx_samp;
}
if (vy_samp <= 0) {
ROS_WARN("You've specified that you don't want any samples in the y dimension. We'll at least assume that you want to sample one value... so we're going to set vy_samples to 1 instead");
vy_samp = 1;
config.vy_samples = vy_samp;
}
if (vth_samp <= 0) {
ROS_WARN("You've specified that you don't want any samples in the th dimension. We'll at least assume that you want to sample one value... so we're going to set vth_samples to 1 instead");
vth_samp = 1;
config.vth_samples = vth_samp;
}
vsamples_[0] = vx_samp;
vsamples_[1] = vy_samp;
vsamples_[2] = vth_samp;
}
DWAPlanner::DWAPlanner(std::string name, base_local_planner::LocalPlannerUtil *planner_util) :
planner_util_(planner_util),
obstacle_costs_(planner_util->getCostmap()),
path_costs_(planner_util->getCostmap(), 0.0, 0.0, false, base_local_planner::Last),
goal_costs_(planner_util->getCostmap(), 0.0, 0.0, true, base_local_planner::Last),
goal_front_costs_(planner_util->getCostmap(), 0.0, 0.0, true, base_local_planner::Last),
alignment_costs_(planner_util->getCostmap(), 0.0, 0.0, false, base_local_planner::Last),
additional_goal_costs_(planner_util->getCostmap(), 0.0, 0.0, false, base_local_planner::Last)
{
ros::NodeHandle private_nh("~/" + name);
goal_front_costs_.setStopOnFailure( false );
alignment_costs_.setStopOnFailure( true );
//Assuming this planner is being run within the navigation stack, we can
//just do an upward search for the frequency at which its being run. This
//also allows the frequency to be overwritten locally.
std::string controller_frequency_param_name;
if(!private_nh.searchParam("controller_frequency", controller_frequency_param_name)) {
sim_period_ = 0.05;
} else {
double controller_frequency = 0;
private_nh.param(controller_frequency_param_name, controller_frequency, 20.0);
if(controller_frequency > 0) {
sim_period_ = 1.0 / controller_frequency;
} else {
ROS_WARN("A controller_frequency less than 0 has been set. Ignoring the parameter, assuming a rate of 20Hz");
sim_period_ = 0.05;
}
}
ROS_INFO("Sim period is set to %.2f", sim_period_);
oscillation_costs_.resetOscillationFlags();
bool sum_scores;
private_nh.param("sum_scores", sum_scores, false);
obstacle_costs_.setSumScores(sum_scores);
private_nh.param("publish_cost_grid_pc", publish_cost_grid_pc_, false);
map_viz_.initialize(name, planner_util->getGlobalFrame(), boost::bind(&DWAPlanner::getCellCosts, this, _1, _2, _3, _4, _5, _6));
std::string frame_id;
private_nh.param("global_frame_id", frame_id, std::string("odom"));
traj_cloud_ = new pcl::PointCloud<base_local_planner::MapGridCostPoint>;
traj_cloud_->header.frame_id = frame_id;
traj_cloud_pub_.advertise(private_nh, "trajectory_cloud", 1);
private_nh.param("publish_traj_pc", publish_traj_pc_, false);
// set up all the cost functions that will be applied in order
// (any function returning negative values will abort scoring, so the order can improve performance)
std::vector<base_local_planner::TrajectoryCostFunction*> critics; //为什么要将所有的cost都要进行压入操作呢，这是因为后面调用的计算代价的函数需要进行迭代相加计算
critics.push_back(&oscillation_costs_); // discards oscillating motions (assisgns cost -1)
critics.push_back(&obstacle_costs_); // discards trajectories that move into obstacles
critics.push_back(&path_costs_); // prefers trajectories on global path
critics.push_back(&goal_costs_); // prefers trajectories that go towards (local) goal, based on wave propagation
private_nh.param("use_goal_front", use_goal_front, true);
if(use_goal_front)
critics.push_back(&goal_front_costs_); // prefers trajectories that make the nose go towards (local) nose goal
private_nh.param("use_alignment", use_alignment, true);
if(use_alignment)
critics.push_back(&alignment_costs_); // prefers trajectories that keep the robot nose on nose path
private_nh.param("use_additional_goal", use_additional_goal, true);
if(use_additional_goal)
critics.push_back(&additional_goal_costs_); // add addtional goal to avoid stuck
// trajectory generators
std::vector<base_local_planner::TrajectorySampleGenerator*> generator_list;//此处压如了一次，因此只有一种生成器，即base_local_planner::SimpleTrajectoryGenerator generator_。
generator_list.push_back(&generator_);
scored_sampling_planner_ = base_local_planner::SimpleScoredSamplingPlanner(generator_list, critics);
private_nh.param("cheat_factor", cheat_factor_, 1.0);
}
// used for visualization only, total_costs are not really total costs
bool DWAPlanner::getCellCosts(int cx, int cy, float &path_cost, float &goal_cost, float &occ_cost, float &total_cost) {
path_cost = path_costs_.getCellCosts(cx, cy);
goal_cost = goal_costs_.getCellCosts(cx, cy);
occ_cost = planner_util_->getCostmap()->getCost(cx, cy);
double goal_front_cost = goal_front_costs_.getCellCosts(cx, cy);
double alignment_path_cost = alignment_costs_.getCellCosts(cx, cy);
double additional_goal_cost = additional_goal_costs_.getCellCosts(cx, cy);
if (path_cost == path_costs_.obstacleCosts() ||
path_cost == path_costs_.unreachableCellCosts() ||
occ_cost >= costmap_2d::INSCRIBED_INFLATED_OBSTACLE) {
return false;
}
double resolution = planner_util_->getCostmap()->getResolution();
total_cost =
pdist_scale_ * resolution * path_cost +
gdist_scale_ * resolution * goal_cost +
occdist_scale_ * occ_cost;
total_cost =
path_costs_.getScale() * path_cost +
goal_costs_.getScale() * goal_cost +
goal_front_costs_.getScale() * goal_front_cost +
alignment_costs_.getScale() * alignment_path_cost +
additional_goal_costs_.getScale() * additional_goal_cost +
obstacle_costs_.getScale() * occ_cost;
return true;
}
bool DWAPlanner::setPlan(const std::vector<geometry_msgs::PoseStamped>& orig_global_plan) {
oscillation_costs_.resetOscillationFlags();
return planner_util_->setPlan(orig_global_plan);
}
/**
* This function is used when other strategies are to be applied,
* but the cost functions for obstacles are to be reused.
*/
bool DWAPlanner::checkTrajectory(
Eigen::Vector3f pos,
Eigen::Vector3f vel,
Eigen::Vector3f vel_samples){
oscillation_costs_.resetOscillationFlags();
base_local_planner::Trajectory traj;
geometry_msgs::PoseStamped goal_pose = global_plan_.back();
Eigen::Vector3f goal(goal_pose.pose.position.x, goal_pose.pose.position.y, tf::getYaw(goal_pose.pose.orientation));
base_local_planner::LocalPlannerLimits limits = planner_util_->getCurrentLimits();
generator_.initialise(pos,
vel,
goal,
&limits,
vsamples_);
generator_.generateTrajectory(pos, vel, vel_samples, traj);
double cost = scored_sampling_planner_.scoreTrajectory(traj, -1);
//if the trajectory is a legal one... the check passes
if(cost >= 0) {
return true;
}
ROS_WARN("Invalid Trajectory %f, %f, %f, cost: %f", vel_samples[0], vel_samples[1], vel_samples[2], cost);
//otherwise the check fails
return false;
}
//in fact, only setTarget to the score functions, we should call prepare to compute cost before using
void DWAPlanner::updatePlanAndLocalCosts(
tf::Stamped<tf::Pose> global_pose,
const std::vector<geometry_msgs::PoseStamped>& new_plan) {
global_plan_.resize(new_plan.size());
for (unsigned int i = 0; i < new_plan.size(); ++i) {
global_plan_[i] = new_plan[i];
}
// costs for going away from path
path_costs_.setTargetPoses(global_plan_);
// costs for not going towards the local goal as much as possible
goal_costs_.setTargetPoses(global_plan_);
// alignment costs
geometry_msgs::PoseStamped goal_pose = global_plan_.back();
Eigen::Vector3f pos(global_pose.getOrigin().getX(), global_pose.getOrigin().getY(), tf::getYaw(global_pose.getRotation()));
//distance between current pose and local goal
double sq_dist =
(pos[0] - goal_pose.pose.position.x) * (pos[0] - goal_pose.pose.position.x) +
(pos[1] - goal_pose.pose.position.y) * (pos[1] - goal_pose.pose.position.y);
// we want the robot nose to be drawn to its final position
// (before robot turns towards goal orientation), not the end of the
// path for the robot center. Choosing the final position after
// turning towards goal orientation causes instability when the
// robot needs to make a 180 degree turn at the end
std::vector<geometry_msgs::PoseStamped> front_global_plan = global_plan_;
double angle_to_goal = atan2(goal_pose.pose.position.y - pos[1], goal_pose.pose.position.x - pos[0]);
front_global_plan.back().pose.position.x = front_global_plan.back().pose.position.x +
forward_point_distance_ * cos(angle_to_goal);
front_global_plan.back().pose.position.y = front_global_plan.back().pose.position.y + forward_point_distance_ *
sin(angle_to_goal);
goal_front_costs_.setTargetPoses(front_global_plan);
//now we'll add some addtional goals for avoiding struck
bool addtional_start = false;
std::vector<geometry_msgs::PoseStamped> addtional_global_plan;
for(int i =0; i < global_plan_.size() ; i++) {
geometry_msgs::PoseStamped goalPose = global_plan_[i];
if(!addtional_start)
{
double dist =
(pos[0] - goalPose.pose.position.x) * (pos[0] - goalPose.pose.position.x) +
(pos[1] - goalPose.pose.position.y) * (pos[1] - goalPose.pose.position.y);
if(dist >= additional_goal_distance_*additional_goal_distance_)
{
addtional_start = true;
}
continue;
}
addtional_global_plan.push_back(goalPose);
}
if(addtional_global_plan.empty())
addtional_global_plan.push_back(goal_pose);
additional_goal_costs_.setTargetPoses(addtional_global_plan);
//if the robot is not near the goal, we using alignment cost; if the robot is very near the goal, we disable the alignment cost.
// keeping the nose on the path
if (sq_dist > forward_point_distance_ * forward_point_distance_ * cheat_factor_) {
double resolution = planner_util_->getCostmap()->getResolution();
alignment_costs_.setScale(resolution * alignment_scale_ * 0.5);
// costs for robot being aligned with path (nose on path, not ju
alignment_costs_.setTargetPoses(global_plan_);
} else {
// once we are close to goal, trying to keep the nose close to anything destabilizes behavior.
alignment_costs_.setScale(0.0);
}
}
/*
* given the current state of the robot, find a good trajectory
*/
base_local_planner::Trajectory DWAPlanner::findBestPath(
tf::Stamped<tf::Pose> global_pose,
tf::Stamped<tf::Pose> global_vel,
tf::Stamped<tf::Pose>& drive_velocities,
std::vector<geometry_msgs::Point> footprint_spec) {
obstacle_costs_.setFootprint(footprint_spec);//footprint内是不考虑障碍物的
//make sure that our configuration doesn't change mid-run
boost::mutex::scoped_lock l(configuration_mutex_);//确保程序在运算的过程中，配置发生变化
Eigen::Vector3f pos(global_pose.getOrigin().getX(), global_pose.getOrigin().getY(), tf::getYaw(global_pose.getRotation()));//当前位姿
Eigen::Vector3f vel(global_vel.getOrigin().getX(), global_vel.getOrigin().getY(), tf::getYaw(global_vel.getRotation()));//当前速度
geometry_msgs::PoseStamped goal_pose = global_plan_.back();//路径的最后一个点设置为目标点
Eigen::Vector3f goal(goal_pose.pose.position.x, goal_pose.pose.position.y, tf::getYaw(goal_pose.pose.orientation));//获取目标位姿
base_local_planner::LocalPlannerLimits limits = planner_util_->getCurrentLimits();//获取各种局部路径规划的参数
// prepare cost functions and generators for this run
generator_.initialise(pos,
vel,
goal,
&limits,
vsamples_);//获取采样速度=======================》simple_trajectory_generator.cpp
result_traj_.cost_ = -7;//刚开始轨迹设为不可行
// find best trajectory by sampling and scoring the samples
std::vector<base_local_planner::Trajectory> all_explored;
scored_sampling_planner_.findBestTrajectory(result_traj_, &all_explored);//===========>发现最优路径。result_traj_就是最优路径，all_explored就是所有的路径是为了后面的显示时使用。
if(publish_traj_pc_)
{
base_local_planner::MapGridCostPoint pt;
traj_cloud_->points.clear();
traj_cloud_->width = 0;
traj_cloud_->height = 0;
std_msgs::Header header;
pcl_conversions::fromPCL(traj_cloud_->header, header);
header.stamp = ros::Time::now();
traj_cloud_->header = pcl_conversions::toPCL(header);
for(std::vector<base_local_planner::Trajectory>::iterator t=all_explored.begin(); t != all_explored.end(); ++t)
{
if(t->cost_<0)
continue;
// Fill out the plan
for(unsigned int i = 0; i < t->getPointsSize(); ++i) {
double p_x, p_y, p_th;
t->getPoint(i, p_x, p_y, p_th);
pt.x=p_x;
pt.y=p_y;
pt.z=0;
pt.path_cost=p_th;
pt.total_cost=t->cost_;
traj_cloud_->push_back(pt);
}
}
traj_cloud_pub_.publish(*traj_cloud_);
}
// verbose publishing of point clouds
if (publish_cost_grid_pc_) {
//we'll publish the visualization of the costs to rviz before returning our best trajectory
map_viz_.publishCostCloud(planner_util_->getCostmap());
}
// debrief stateful scoring functions
oscillation_costs_.updateOscillationFlags(pos, &result_traj_, planner_util_->getCurrentLimits().min_trans_vel);
//if we don't have a legal trajectory, we'll just command zero
if (result_traj_.cost_ < 0) {
drive_velocities.setIdentity();
} else {
tf::Vector3 start(result_traj_.xv_, result_traj_.yv_, 0);
drive_velocities.setOrigin(start);
tf::Matrix3x3 matrix;
matrix.setRotation(tf::createQuaternionFromYaw(result_traj_.thetav_));
drive_velocities.setBasis(matrix);
}
return result_traj_;
}
};dwa_planner_ros.h
#ifndef DWA_LOCAL_PLANNER_DWA_PLANNER_ROS_H_
#define DWA_LOCAL_PLANNER_DWA_PLANNER_ROS_H_
#include <boost/shared_ptr.hpp>
#include <boost/thread.hpp>
#include <tf/transform_listener.h>
#include <dynamic_reconfigure/server.h>
#include <dwa_local_planner/DWAPlannerConfig.h>
#include <angles/angles.h>
#include <nav_msgs/Odometry.h>
#include <costmap_2d/costmap_2d_ros.h>
#include <nav_core/base_local_planner.h>
#include <base_local_planner/latched_stop_rotate_controller.h>
#include <base_local_planner/odometry_helper_ros.h>
#include <dwa_local_planner/dwa_planner.h>
#include <std_msgs/builtin_string.h>
#include <std_msgs/Int32.h>
namespace dwa_local_planner {
/**
* @class DWAPlannerROS
* @brief ROS Wrapper for the DWAPlanner that adheres to the
* BaseLocalPlanner interface and can be used as a plugin for move_base.
*/
class DWAPlannerROS : public nav_core::BaseLocalPlanner {
public:
/**
* @brief Constructor for DWAPlannerROS wrapper
*/
DWAPlannerROS();
/**
* @brief Constructs the ros wrapper
* @param name The name to give this instance of the trajectory planner
* @param tf A pointer to a transform listener
* @param costmap The cost map to use for assigning costs to trajectories
*/
void initialize(std::string name, tf::TransformListener* tf,
costmap_2d::Costmap2DROS* costmap_ros);//初始化
/**
* @brief Destructor for the wrapper
*/
~DWAPlannerROS();
/**
* @brief Given the current position, orientation, and velocity of the robot,
* compute velocity commands to send to the base
* @param cmd_vel Will be filled with the velocity command to be passed to the robot base
* @return True if a valid trajectory was found, false otherwise
*/
bool computeVelocityCommands(geometry_msgs::Twist& cmd_vel);//计算控制速度
/**
* @brief Given the current position, orientation, and velocity of the robot,
* compute velocity commands to send to the base, using dynamic window approach
* @param cmd_vel Will be filled with the velocity command to be passed to the robot base
* @return True if a valid trajectory was found, false otherwise
*/
bool dwaComputeVelocityCommands(tf::Stamped<tf::Pose>& global_pose, geometry_msgs::Twist& cmd_vel);
/**
* @brief Set the plan that the controller is following
* @param orig_global_plan The plan to pass to the controller
* @return True if the plan was updated successfully, false otherwise
*/
bool setPlan(const std::vector<geometry_msgs::PoseStamped>& orig_global_plan);//设置全局路径
/**
* @brief Check if the goal pose has been achieved
* @return True if achieved, false otherwise
*/
bool isGoalReached();//判断是否到达目标点
bool isInitialized() {
return initialized_;
}
private:
/**
* @brief Callback to update the local planner's parameters based on dynamic reconfigure
*/
void reconfigureCB(DWAPlannerConfig &config, uint32_t level);
void publishLocalPlan(std::vector<geometry_msgs::PoseStamped>& path);
void publishGlobalPlan(std::vector<geometry_msgs::PoseStamped>& path);
tf::TransformListener* tf_; ///< @brief Used for transforming point clouds
// for visualisation, publishers of global and local plan
ros::Publisher g_plan_pub_, l_plan_pub_;
base_local_planner::LocalPlannerUtil planner_util_;//存储运动控制参数以及costmap2d,tf等，会被传入dp_
boost::shared_ptr<DWAPlanner> dp_; ///< @brief The trajectory controller,dwa运动控制类
costmap_2d::Costmap2DROS* costmap_ros_;//地图
dynamic_reconfigure::Server<DWAPlannerConfig> *dsrv_;
dwa_local_planner::DWAPlannerConfig default_config_;
bool setup_;
tf::Stamped<tf::Pose> current_pose_;
base_local_planner::LatchedStopRotateController latchedStopRotateController_;// 到达目标点后的停止旋转运动控制类
bool initialized_;
base_local_planner::OdometryHelperRos odom_helper_;//用来获取odom信息，传入dp_
std::string odom_topic_;
ros::Publisher dwaPub_;
std_msgs::String dwa_msg;
};
};
#endif
dwa_planner_ros.cpp
#include <dwa_local_planner/dwa_planner_ros.h>
#include <Eigen/Core>
#include <cmath>
#include <ros/console.h>
#include <pluginlib/class_list_macros.h>
#include <base_local_planner/goal_functions.h>
#include <nav_msgs/Path.h>
//register this planner as a BaseLocalPlanner plugin
PLUGINLIB_EXPORT_CLASS(dwa_local_planner::DWAPlannerROS, nav_core::BaseLocalPlanner)
namespace dwa_local_planner {
void DWAPlannerROS::reconfigureCB(DWAPlannerConfig &config, uint32_t level) {
if (setup_ && config.restore_defaults) {
config = default_config_;
config.restore_defaults = false;
}
if ( ! setup_) {
default_config_ = config;
setup_ = true;
}
// update generic local planner params
base_local_planner::LocalPlannerLimits limits;
limits.max_trans_vel = config.max_trans_vel;
limits.min_trans_vel = config.min_trans_vel;
limits.max_vel_x = config.max_vel_x;
limits.min_vel_x = config.min_vel_x;
limits.max_vel_y = config.max_vel_y;
limits.min_vel_y = config.min_vel_y;
limits.max_rot_vel = config.max_rot_vel;
limits.min_rot_vel = config.min_rot_vel;
limits.acc_lim_x = config.acc_lim_x;
limits.acc_lim_y = config.acc_lim_y;
limits.acc_lim_theta = config.acc_lim_theta;
limits.acc_limit_trans = config.acc_limit_trans;
limits.xy_goal_tolerance = config.xy_goal_tolerance;
limits.yaw_goal_tolerance = config.yaw_goal_tolerance;
limits.prune_plan = config.prune_plan;
limits.trans_stopped_vel = config.trans_stopped_vel;
limits.rot_stopped_vel = config.rot_stopped_vel;
planner_util_.reconfigureCB(limits, config.restore_defaults);
// update dwa specific configuration
dp_->reconfigure(config);
}
DWAPlannerROS::DWAPlannerROS() : initialized_(false),
odom_helper_("odom"), setup_(false) {
}
void DWAPlannerROS::initialize(
std::string name,
tf::TransformListener* tf,
costmap_2d::Costmap2DROS* costmap_ros) {
if (! isInitialized()) {
ros::NodeHandle private_nh("~/" + name);
g_plan_pub_ = private_nh.advertise<nav_msgs::Path>("global_plan", 1);
l_plan_pub_ = private_nh.advertise<nav_msgs::Path>("local_plan", 1);
dwaPub_ = private_nh.advertise<std_msgs::String>("/dwa_file", 2);
tf_ = tf;
costmap_ros_ = costmap_ros;
costmap_ros_->getRobotPose(current_pose_);
// make sure to update the costmap we'll use for this cycle
costmap_2d::Costmap2D* costmap = costmap_ros_->getCostmap();
planner_util_.initialize(tf, costmap, costmap_ros_->getGlobalFrameID());//costmap的初始化处理
//create the actual planner that we'll use.. it'll configure itself from the parameter server
dp_ = boost::shared_ptr<DWAPlanner>(new DWAPlanner(name, &planner_util_));
if( private_nh.getParam( "odom_topic", odom_topic_ ))
{
odom_helper_.setOdomTopic( odom_topic_ );
}
initialized_ = true;
dsrv_ = new dynamic_reconfigure::Server<DWAPlannerConfig>(private_nh);
dynamic_reconfigure::Server<DWAPlannerConfig>::CallbackType cb = boost::bind(&DWAPlannerROS::reconfigureCB, this, _1, _2);
dsrv_->setCallback(cb);
}
else{
ROS_WARN("This planner has already been initialized, doing nothing.");
}
}
bool DWAPlannerROS::setPlan(const std::vector<geometry_msgs::PoseStamped>& orig_global_plan) {
if (! isInitialized()) {
ROS_ERROR("This planner has not been initialized, please call initialize() before using this planner");
return false;
}
//when we get a new plan, we also want to clear any latch we may have on goal tolerances
latchedStopRotateController_.resetLatching();
ROS_INFO("Got new plan");
dwa_msg.data = "Dynamic_path_planning_start.wav";
dwaPub_.publish(dwa_msg);
return dp_->setPlan(orig_global_plan);//将全局路径输入
}
bool DWAPlannerROS::isGoalReached() {//判断是否已经到达目标位置
if (! isInitialized()) {
ROS_ERROR("This planner has not been initialized, please call initialize() before using this planner");
return false;
}
if ( ! costmap_ros_->getRobotPose(current_pose_)) {
ROS_ERROR("Could not get robot pose");
return false;
}
if(latchedStopRotateController_.isGoalReached(&planner_util_, odom_helper_, current_pose_)) {
ROS_INFO("Goal reached");
dwa_msg.data = "The_goal_has_arrived.wav";
dwaPub_.publish(dwa_msg);
return true;
} else {
return false;
}
}
void DWAPlannerROS::publishLocalPlan(std::vector<geometry_msgs::PoseStamped>& path) {
base_local_planner::publishPlan(path, l_plan_pub_);
}
void DWAPlannerROS::publishGlobalPlan(std::vector<geometry_msgs::PoseStamped>& path) {
base_local_planner::publishPlan(path, g_plan_pub_);
}
DWAPlannerROS::~DWAPlannerROS(){
//make sure to clean things up
delete dsrv_;
}
bool DWAPlannerROS::dwaComputeVelocityCommands(tf::Stamped<tf::Pose> &global_pose, geometry_msgs::Twist& cmd_vel) {
// dynamic window sampling approach to get useful velocity commands
if(! isInitialized()){
ROS_ERROR("This planner has not been initialized, please call initialize() before using this planner");
return false;
}
tf::Stamped<tf::Pose> robot_vel;
odom_helper_.getRobotVel(robot_vel);
//For timing uncomment
struct timeval start, end;
double start_t, end_t, t_diff;
gettimeofday(&start, NULL);
//compute what trajectory to drive along
tf::Stamped<tf::Pose> drive_cmds;
drive_cmds.frame_id_ = costmap_ros_->getBaseFrameID();
// call with updated footprint
base_local_planner::Trajectory path = dp_->findBestPath(global_pose, robot_vel, drive_cmds, costmap_ros_->getRobotFootprint());//=================》核心调用，发现最优路径，其中globalpose是机器人的位姿，robotvel是根据odom获取的速度，drivecmds为产生的最优速度，最后的参数是机器人的footprint
//ROS_ERROR("Best: %.2f, %.2f, %.2f, %.2f", path.xv_, path.yv_, path.thetav_, path.cost_);
//For timing uncomment
gettimeofday(&end, NULL);
start_t = start.tv_sec + double(start.tv_usec) / 1e6;
end_t = end.tv_sec + double(end.tv_usec) / 1e6;
t_diff = end_t - start_t;
ROS_DEBUG_NAMED("dwa_local_planner","Cycle time: %.9f", t_diff);
//pass along drive commands
cmd_vel.linear.x = drive_cmds.getOrigin().getX();//对规划出的速度进行填充，可以看出上面的findbestpath产生的就是速度，并没有产生加速度。只是速度的变化，导致产生了加速度而已。
cmd_vel.linear.y = drive_cmds.getOrigin().getY();
cmd_vel.angular.z = tf::getYaw(drive_cmds.getRotation());
//if we cannot move... tell someone
std::vector<geometry_msgs::PoseStamped> local_plan;
if(path.cost_ < 0) {
ROS_DEBUG_NAMED("dwa_local_planner",
"The dwa local planner failed to find a valid plan, cost functions discarded all candidates. This can mean there is an obstacle too close to the robot.");
local_plan.clear();
publishLocalPlan(local_plan);
return false;
}
ROS_DEBUG_NAMED("dwa_local_planner", "A valid velocity command of (%.2f, %.2f, %.2f) was found for this cycle.",
cmd_vel.linear.x, cmd_vel.linear.y, cmd_vel.angular.z);
// Fill out the local plan
for(unsigned int i = 0; i < path.getPointsSize(); ++i) {
double p_x, p_y, p_th;
path.getPoint(i, p_x, p_y, p_th);
tf::Stamped<tf::Pose> p =
tf::Stamped<tf::Pose>(tf::Pose(
tf::createQuaternionFromYaw(p_th),
tf::Point(p_x, p_y, 0.0)),
ros::Time::now(),
costmap_ros_->getGlobalFrameID());
geometry_msgs::PoseStamped pose;
tf::poseStampedTFToMsg(p, pose);
local_plan.push_back(pose);
}
//publish information to the visualizer
publishLocalPlan(local_plan);//将最好的规划路径publish出去
return true;
}
bool DWAPlannerROS::computeVelocityCommands(geometry_msgs::Twist& cmd_vel) {
// dispatches to either dwa sampling control or stop and rotate control, depending on whether we have been close enough to goal
if ( ! costmap_ros_->getRobotPose(current_pose_)) {
ROS_ERROR("Could not get robot pose");
return false;
}
std::vector<geometry_msgs::PoseStamped> transformed_plan;
if ( ! planner_util_.getLocalPlan(current_pose_, transformed_plan)) {//=====================》将全局路径根据机器人的位置获取要跟踪的局部路径
ROS_ERROR("Could not get local plan");
return false;
}
//if the global plan passed in is empty... we won't do anything
if(transformed_plan.empty()) {
ROS_WARN_NAMED("dwa_local_planner", "Received an empty transformed plan.");
return false;
}
ROS_DEBUG_NAMED("dwa_local_planner", "Received a transformed plan with %zu points.", transformed_plan.size());
// update plan in dwa_planner even if we just stop and rotate, to allow checkTrajectory
dp_->updatePlanAndLocalCosts(current_pose_, transformed_plan);//对局部路径进行更新，在旋转停止等情况下
if (latchedStopRotateController_.isPositionReached(&planner_util_, current_pose_)) {//判断位置上是否已经到达目标
//publish an empty plan because we've reached our goal position
std::vector<geometry_msgs::PoseStamped> local_plan;
std::vector<geometry_msgs::PoseStamped> transformed_plan;
publishGlobalPlan(transformed_plan);
publishLocalPlan(local_plan);//两个plan都是空的
base_local_planner::LocalPlannerLimits limits = planner_util_.getCurrentLimits();
return latchedStopRotateController_.computeVelocityCommandsStopRotate(//计算达到目标位置后的旋转以调整位姿
cmd_vel,
limits.getAccLimits(),
dp_->getSimPeriod(),
&planner_util_,
odom_helper_,
current_pose_,
boost::bind(&DWAPlanner::checkTrajectory, dp_, _1, _2, _3));
} else {
bool isOk = dwaComputeVelocityCommands(current_pose_, cmd_vel);
if (isOk) {
publishGlobalPlan(transformed_plan);//此处发布跟踪的全局路径
} else {
ROS_WARN_NAMED("dwa_local_planner", "DWA planner failed to produce path.");
std::vector<geometry_msgs::PoseStamped> empty_plan;
publishGlobalPlan(empty_plan);
}
return isOk;
}
}
};goal_function.cpp
#include <base_local_planner/goal_functions.h>
namespace base_local_planner {
double getGoalPositionDistance(const tf::Stamped<tf::Pose>& global_pose, double goal_x, double goal_y) {
return hypot(goal_x - global_pose.getOrigin().x(), goal_y - global_pose.getOrigin().y());
}
double getGoalOrientationAngleDifference(const tf::Stamped<tf::Pose>& global_pose, double goal_th) {
double yaw = tf::getYaw(global_pose.getRotation());
return angles::shortest_angular_distance(yaw, goal_th);
}
void publishPlan(const std::vector<geometry_msgs::PoseStamped>& path, const ros::Publisher& pub) {
//given an empty path we won't do anything
if(path.empty())
return;
//create a path message
nav_msgs::Path gui_path;
gui_path.poses.resize(path.size());
gui_path.header.frame_id = path[0].header.frame_id;
gui_path.header.stamp = path[0].header.stamp;
// Extract the plan in world co-ordinates, we assume the path is all in the same frame
for(unsigned int i=0; i < path.size(); i++){
gui_path.poses[i] = path[i];
}
pub.publish(gui_path);
}
void prunePlan(const tf::Stamped<tf::Pose>& global_pose, std::vector<geometry_msgs::PoseStamped>& plan, std::vector<geometry_msgs::PoseStamped>& global_plan){
ROS_ASSERT(global_plan.size() >= plan.size());//保证裁剪全局路径是大于局部路径的
std::vector<geometry_msgs::PoseStamped>::iterator it = plan.begin();
std::vector<geometry_msgs::PoseStamped>::iterator global_it = global_plan.begin();
while(it != plan.end()){
const geometry_msgs::PoseStamped& w = *it;
// Fixed error bound of 2 meters for now. Can reduce to a portion of the map size or based on the resolution
double x_diff = global_pose.getOrigin().x() - w.pose.position.x;
double y_diff = global_pose.getOrigin().y() - w.pose.position.y;
double distance_sq = x_diff * x_diff + y_diff * y_diff;
if(distance_sq < 1){
ROS_DEBUG("Nearest waypoint to <%f, %f> is <%f, %f>\n", global_pose.getOrigin().x(), global_pose.getOrigin().y(), w.pose.position.x, w.pose.position.y);
break;
}
it = plan.erase(it);
global_it = global_plan.erase(global_it);
}
}
bool transformGlobalPlan(
const tf::TransformListener& tf,
const std::vector<geometry_msgs::PoseStamped>& global_plan,
const tf::Stamped<tf::Pose>& global_pose,
const costmap_2d::Costmap2D& costmap,
const std::string& global_frame,
std::vector<geometry_msgs::PoseStamped>& transformed_plan){//将全局路径进行最表转换转换到局部的跟踪路径
transformed_plan.clear();
if (global_plan.empty()) {
ROS_ERROR("Received plan with zero length");
return false;
}
const geometry_msgs::PoseStamped& plan_pose = global_plan[0];
try {
// get plan_to_global_transform from plan frame to global_frame
tf::StampedTransform plan_to_global_transform;
tf.waitForTransform(global_frame, ros::Time::now(),
plan_pose.header.frame_id, plan_pose.header.stamp,
plan_pose.header.frame_id, ros::Duration(0.5));
tf.lookupTransform(global_frame, ros::Time(),
plan_pose.header.frame_id, plan_pose.header.stamp,
plan_pose.header.frame_id, plan_to_global_transform);
//let's get the pose of the robot in the frame of the plan
tf::Stamped<tf::Pose> robot_pose;
tf.transformPose(plan_pose.header.frame_id, global_pose, robot_pose);//获取机器人在局部规划的位姿
//we'll discard points on the plan that are outside the local costmap
double dist_threshold = std::max(costmap.getSizeInCellsX() * costmap.getResolution() / 2.0,
costmap.getSizeInCellsY() * costmap.getResolution() / 2.0);
unsigned int i = 0;
double sq_dist_threshold = dist_threshold * dist_threshold;
double sq_dist = 0;
//we need to loop to a point on the plan that is within a certain distance of the robot
while(i < (unsigned int)global_plan.size()) {//将在局部地图之外的路径消除掉
double x_diff = robot_pose.getOrigin().x() - global_plan[i].pose.position.x;
double y_diff = robot_pose.getOrigin().y() - global_plan[i].pose.position.y;
sq_dist = x_diff * x_diff + y_diff * y_diff;
if (sq_dist <= sq_dist_threshold) {
break;
}
++i;
}
tf::Stamped<tf::Pose> tf_pose;
geometry_msgs::PoseStamped newer_pose;
//now we'll transform until points are outside of our distance threshold
while(i < (unsigned int)global_plan.size() && sq_dist <= sq_dist_threshold) {
const geometry_msgs::PoseStamped& pose = global_plan[i];
poseStampedMsgToTF(pose, tf_pose);
tf_pose.setData(plan_to_global_transform * tf_pose);
tf_pose.stamp_ = plan_to_global_transform.stamp_;
tf_pose.frame_id_ = global_frame;
poseStampedTFToMsg(tf_pose, newer_pose);
transformed_plan.push_back(newer_pose);
double x_diff = robot_pose.getOrigin().x() - global_plan[i].pose.position.x;
double y_diff = robot_pose.getOrigin().y() - global_plan[i].pose.position.y;
sq_dist = x_diff * x_diff + y_diff * y_diff;
++i;
}
}
catch(tf::LookupException& ex) {
ROS_ERROR("No Transform available Error: %s\n", ex.what());
return false;
}
catch(tf::ConnectivityException& ex) {
ROS_ERROR("Connectivity Error: %s\n", ex.what());
return false;
}
catch(tf::ExtrapolationException& ex) {
ROS_ERROR("Extrapolation Error: %s\n", ex.what());
if (!global_plan.empty())
ROS_ERROR("Global Frame: %s Plan Frame size %d: %s\n", global_frame.c_str(), (unsigned int)global_plan.size(), global_plan[0].header.frame_id.c_str());
return false;
}
return true;
}
bool getGoalPose(const tf::TransformListener& tf,
const std::vector<geometry_msgs::PoseStamped>& global_plan,
const std::string& global_frame, tf::Stamped<tf::Pose>& goal_pose) {
if (global_plan.empty())
{
ROS_ERROR("Received plan with zero length");
return false;
}
const geometry_msgs::PoseStamped& plan_goal_pose = global_plan.back();
try{
tf::StampedTransform transform;
tf.waitForTransform(global_frame, ros::Time::now(),
plan_goal_pose.header.frame_id, plan_goal_pose.header.stamp,
plan_goal_pose.header.frame_id, ros::Duration(0.5));
tf.lookupTransform(global_frame, ros::Time(),
plan_goal_pose.header.frame_id, plan_goal_pose.header.stamp,
plan_goal_pose.header.frame_id, transform);
poseStampedMsgToTF(plan_goal_pose, goal_pose);
goal_pose.setData(transform * goal_pose);
goal_pose.stamp_ = transform.stamp_;
goal_pose.frame_id_ = global_frame;
}
catch(tf::LookupException& ex) {
ROS_ERROR("No Transform available Error: %s\n", ex.what());
return false;
}
catch(tf::ConnectivityException& ex) {
ROS_ERROR("Connectivity Error: %s\n", ex.what());
return false;
}
catch(tf::ExtrapolationException& ex) {
ROS_ERROR("Extrapolation Error: %s\n", ex.what());
if (global_plan.size() > 0)
ROS_ERROR("Global Frame: %s Plan Frame size %d: %s\n", global_frame.c_str(), (unsigned int)global_plan.size(), global_plan[0].header.frame_id.c_str());
return false;
}
return true;
}
bool isGoalReached(const tf::TransformListener& tf,
const std::vector<geometry_msgs::PoseStamped>& global_plan,
const costmap_2d::Costmap2D& costmap __attribute__((unused)),
const std::string& global_frame,
tf::Stamped<tf::Pose>& global_pose,
const nav_msgs::Odometry& base_odom,
double rot_stopped_vel, double trans_stopped_vel,
double xy_goal_tolerance, double yaw_goal_tolerance){
//we assume the global goal is the last point in the global plan
tf::Stamped<tf::Pose> goal_pose;
getGoalPose(tf, global_plan, global_frame, goal_pose);
double goal_x = goal_pose.getOrigin().getX();
double goal_y = goal_pose.getOrigin().getY();
double goal_th = tf::getYaw(goal_pose.getRotation());
//check to see if we've reached the goal position
if(getGoalPositionDistance(global_pose, goal_x, goal_y) <= xy_goal_tolerance) {
//check to see if the goal orientation has been reached
if(fabs(getGoalOrientationAngleDifference(global_pose, goal_th)) <= yaw_goal_tolerance) {
//make sure that we're actually stopped before returning success
if(stopped(base_odom, rot_stopped_vel, trans_stopped_vel))
return true;
}
}
return false;
}
bool stopped(const nav_msgs::Odometry& base_odom,
const double& rot_stopped_velocity, const double& trans_stopped_velocity){
return fabs(base_odom.twist.twist.angular.z) <= rot_stopped_velocity
&& fabs(base_odom.twist.twist.linear.x) <= trans_stopped_velocity
&& fabs(base_odom.twist.twist.linear.y) <= trans_stopped_velocity;
}
};local_planner_util.cpp
#include <base_local_planner/local_planner_util.h>
#include <base_local_planner/goal_functions.h>
namespace base_local_planner {
void LocalPlannerUtil::initialize(
tf::TransformListener* tf,
costmap_2d::Costmap2D* costmap,
std::string global_frame) {
if(!initialized_) {
tf_ = tf;
costmap_ = costmap;
global_frame_ = global_frame;
initialized_ = true;
}
else{
ROS_WARN("Planner utils have already been initialized, doing nothing.");
}
}
void LocalPlannerUtil::reconfigureCB(LocalPlannerLimits &config, bool restore_defaults)
{
if(setup_ && restore_defaults) {
config = default_limits_;
}
if(!setup_) {
default_limits_ = config;
setup_ = true;
}
boost::mutex::scoped_lock l(limits_configuration_mutex_);
limits_ = LocalPlannerLimits(config);
}
costmap_2d::Costmap2D* LocalPlannerUtil::getCostmap() {
return costmap_;
}
LocalPlannerLimits LocalPlannerUtil::getCurrentLimits() {
boost::mutex::scoped_lock l(limits_configuration_mutex_);
return limits_;
}
bool LocalPlannerUtil::getGoal(tf::Stamped<tf::Pose>& goal_pose) {
//we assume the global goal is the last point in the global plan
return base_local_planner::getGoalPose(*tf_,
global_plan_,
global_frame_,
goal_pose);
}
bool LocalPlannerUtil::setPlan(const std::vector<geometry_msgs::PoseStamped>& orig_global_plan) {
if(!initialized_){
ROS_ERROR("Planner utils have not been initialized, please call initialize() first");
return false;
}
//reset the global plan
global_plan_.clear();
global_plan_ = orig_global_plan;
return true;
}
bool LocalPlannerUtil::getLocalPlan(tf::Stamped<tf::Pose>& global_pose, std::vector<geometry_msgs::PoseStamped>& transformed_plan) {//将全局路径根据机器人的位置获取要跟踪的局部路径
//get the global plan in our frame
if(!base_local_planner::transformGlobalPlan(
*tf_,
global_plan_,
global_pose,
*costmap_,//注意此处的costmap指的是局部地图
global_frame_,
transformed_plan)) {
ROS_WARN("Could not transform the global plan to the frame of the controller");
return false;
}
//now we'll prune the plan based on the position of the robot
if(limits_.prune_plan) {
base_local_planner::prunePlan(global_pose, transformed_plan, global_plan_);
}
return true;
}
}map_grid.h
#ifndef TRAJECTORY_ROLLOUT_MAP_GRID_H_
#define TRAJECTORY_ROLLOUT_MAP_GRID_H_
#include <vector>
#include <iostream>
#include <base_local_planner/trajectory_inc.h>
#include <ros/console.h>
#include <ros/ros.h>
#include <base_local_planner/map_cell.h>
#include <costmap_2d/costmap_2d.h>
#include <geometry_msgs/PoseStamped.h>
namespace base_local_planner{
/**
* @class MapGrid
* @brief A grid of MapCell cells that is used to propagate path and goal distances for the trajectory controller.
*/
class MapGrid{
public:
/**
* @brief Creates a 0x0 map by default
*/
MapGrid();
/**
* @brief Creates a map of size_x by size_y
* @param size_x The width of the map
* @param size_y The height of the map
*/
MapGrid(unsigned int size_x, unsigned int size_y);
/**
* @brief Returns a map cell accessed by (col, row)
* @param x The x coordinate of the cell
* @param y The y coordinate of the cell
* @return A reference to the desired cell
*/
inline MapCell& operator() (unsigned int x, unsigned int y){//返回的是一个mapcell类型，返回的数据为map_的一个点
return map_[size_x_ * y + x];//找出map所在的map点：列数*列的点的个数+点所在的行数 //注意此处的map_成员变量的定义即，如何初始化的。
}
/**
* @brief Returns a map cell accessed by (col, row)
* @param x The x coordinate of the cell
* @param y The y coordinate of the cell
* @return A copy of the desired cell
*/
inline MapCell operator() (unsigned int x, unsigned int y) const {
return map_[size_x_ * y + x];
}
inline MapCell& getCell(unsigned int x, unsigned int y){
return map_[size_x_ * y + x];
}
/**
* @brief Destructor for a MapGrid
*/
~MapGrid(){}
/**
* @brief Copy constructor for a MapGrid
* @param mg The MapGrid to copy
*/
MapGrid(const MapGrid& mg);
/**
* @brief Assignment operator for a MapGrid
* @param mg The MapGrid to assign from
*/
MapGrid& operator= (const MapGrid& mg);
/**
* @brief reset path distance fields for all cells
*/
void resetPathDist();
/**
* @brief check if we need to resize
* @param size_x The desired width
* @param size_y The desired height
*/
void sizeCheck(unsigned int size_x, unsigned int size_y);
/**
* @brief Utility to share initialization code across constructors
*/
void commonInit();
/**
* @brief Returns a 1D index into the MapCell array for a 2D index
* @param x The desired x coordinate
* @param y The desired y coordinate
* @return The associated 1D index
*/
size_t getIndex(int x, int y);
/**
* return a value that indicates cell is in obstacle
*/
inline double obstacleCosts() {
return map_.size();
}
/**
* returns a value indicating cell was not reached by wavefront
* propagation of set cells. (is behind walls, regarding the region covered by grid)
*/
inline double unreachableCellCosts() {
return map_.size() + 1;
}
/**
* @brief Used to update the distance of a cell in path distance computation
* @param current_cell The cell we're currently in
* @param check_cell The cell to be updated
*/
inline bool updatePathCell(MapCell* current_cell, MapCell* check_cell,
const costmap_2d::Costmap2D& costmap);
/**
* increase global plan resolution to match that of the costmap by adding points linearly between global plan points
* This is necessary where global planners produce plans with few points.
* @param global_plan_in input
* @param global_plan_output output
* @param resolution desired distance between waypoints
*/
static void adjustPlanResolution(const std::vector<geometry_msgs::PoseStamped>& global_plan_in,
std::vector<geometry_msgs::PoseStamped>& global_plan_out, double resolution);
/**
* @brief Compute the distance from each cell in the local map grid to the planned path
* @param dist_queue A queue of the initial cells on the path
*/
void computeTargetDistance(std::queue<MapCell*>& dist_queue, const costmap_2d::Costmap2D& costmap);
/**
* @brief Compute the distance from each cell in the local map grid to the local goal point
* @param goal_queue A queue containing the local goal cell
*/
void computeGoalDistance(std::queue<MapCell*>& dist_queue, const costmap_2d::Costmap2D& costmap);
/**
* @brief Update what cells are considered path based on the global plan
*/
void setTargetCells(const costmap_2d::Costmap2D& costmap, const std::vector<geometry_msgs::PoseStamped>& global_plan);
/**
* @brief Update what cell is considered the next local goal
*/
void setLocalGoal(const costmap_2d::Costmap2D& costmap,
const std::vector<geometry_msgs::PoseStamped>& global_plan);
double goal_x_, goal_y_; /**< @brief The goal distance was last computed from */
unsigned int size_x_, size_y_; ///< @brief The dimensions of the grid
private:
std::vector<MapCell> map_; ///< @brief Storage for the MapCells
};
};
#endif
map_grid.cpp
#include <base_local_planner/map_grid.h>
#include <costmap_2d/cost_values.h>
using namespace std;
namespace base_local_planner{
MapGrid::MapGrid()
: size_x_(0), size_y_(0)
{
}
MapGrid::MapGrid(unsigned int size_x, unsigned int size_y)
: size_x_(size_x), size_y_(size_y)
{
commonInit();
}
MapGrid::MapGrid(const MapGrid& mg){
size_y_ = mg.size_y_;
size_x_ = mg.size_x_;
map_ = mg.map_;
}
void MapGrid::commonInit(){
//don't allow construction of zero size grid
ROS_ASSERT(size_y_ != 0 && size_x_ != 0);
map_.resize(size_y_ * size_x_);
//make each cell aware of its location in the grid
for(unsigned int i = 0; i < size_y_; ++i){
for(unsigned int j = 0; j < size_x_; ++j){
unsigned int id = size_x_ * i + j;
map_[id].cx = j;
map_[id].cy = i;
}
}
}
size_t MapGrid::getIndex(int x, int y){
return size_x_ * y + x;
}
MapGrid& MapGrid::operator= (const MapGrid& mg){
size_y_ = mg.size_y_;
size_x_ = mg.size_x_;
map_ = mg.map_;
return *this;
}
void MapGrid::sizeCheck(unsigned int size_x, unsigned int size_y){
if(map_.size() != size_x * size_y)
map_.resize(size_x * size_y);
if(size_x_ != size_x || size_y_ != size_y){
size_x_ = size_x;
size_y_ = size_y;
for(unsigned int i = 0; i < size_y_; ++i){
for(unsigned int j = 0; j < size_x_; ++j){
int index = size_x_ * i + j;
map_[index].cx = j;
map_[index].cy = i;
}
}
}
}
inline bool MapGrid::updatePathCell(MapCell* current_cell, MapCell* check_cell,
const costmap_2d::Costmap2D& costmap){
//if the cell is an obstacle set the max path distance
unsigned char cost = costmap.getCost(check_cell->cx, check_cell->cy);
if(! getCell(check_cell->cx, check_cell->cy).within_robot &&
(cost == costmap_2d::LETHAL_OBSTACLE ||
cost == costmap_2d::INSCRIBED_INFLATED_OBSTACLE ||
cost == costmap_2d::NO_INFORMATION)){
check_cell->target_dist = obstacleCosts();
return false;
}
double new_target_dist = current_cell->target_dist + 1;//由于其相对于 current_cell远离了路径1个格，所以此处加1
if (new_target_dist < check_cell->target_dist) {
check_cell->target_dist = new_target_dist;
}
return true;
}
//reset the path_dist and goal_dist fields for all cells
void MapGrid::resetPathDist(){
for(unsigned int i = 0; i < map_.size(); ++i) {
map_[i].target_dist = unreachableCellCosts();
map_[i].target_mark = false;
map_[i].within_robot = false;
}
}
void MapGrid::adjustPlanResolution(const std::vector<geometry_msgs::PoseStamped>& global_plan_in,
std::vector<geometry_msgs::PoseStamped>& global_plan_out, double resolution) {
if (global_plan_in.size() == 0) {
return;
}
double last_x = global_plan_in[0].pose.position.x;
double last_y = global_plan_in[0].pose.position.y;
global_plan_out.push_back(global_plan_in[0]);
// we can take "holes" in the plan smaller than 2 grid cells (squared = 4)
double min_sq_resolution = resolution * resolution * 4;
for (unsigned int i = 1; i < global_plan_in.size(); ++i) {
double loop_x = global_plan_in[i].pose.position.x;
double loop_y = global_plan_in[i].pose.position.y;
double sqdist = (loop_x - last_x) * (loop_x - last_x) + (loop_y - last_y) * (loop_y - last_y);
if (sqdist > min_sq_resolution) {
int steps = ((sqrt(sqdist) - sqrt(min_sq_resolution)) / resolution) - 1;
// add a points in-between
double deltax = (loop_x - last_x) / steps;
double deltay = (loop_y - last_y) / steps;
// TODO: Interpolate orientation
for (int j = 1; j < steps; ++j) {
geometry_msgs::PoseStamped pose;
pose.pose.position.x = last_x + j * deltax;
pose.pose.position.y = last_y + j * deltay;
pose.pose.position.z = global_plan_in[i].pose.position.z;
pose.pose.orientation = global_plan_in[i].pose.orientation;
pose.header = global_plan_in[i].header;
global_plan_out.push_back(pose);
}
}
global_plan_out.push_back(global_plan_in[i]);
last_x = loop_x;
last_y = loop_y;
}
}
//update what map cells are considered path based on the global_plan
void MapGrid::setTargetCells(const costmap_2d::Costmap2D& costmap,//得到进行局部路径规划需要考虑的全局路径
const std::vector<geometry_msgs::PoseStamped>& global_plan) {
sizeCheck(costmap.getSizeInCellsX(), costmap.getSizeInCellsY());
bool started_path = false;
queue<MapCell*> path_dist_queue;
std::vector<geometry_msgs::PoseStamped> adjusted_global_plan;
adjustPlanResolution(global_plan, adjusted_global_plan, costmap.getResolution());
if (adjusted_global_plan.size() != global_plan.size()) {
ROS_DEBUG("Adjusted global plan resolution, added %zu points", adjusted_global_plan.size() - global_plan.size());
}
unsigned int i;
// put global path points into local map until we reach the border of the local map
for (i = 0; i < adjusted_global_plan.size(); ++i) {
double g_x = adjusted_global_plan[i].pose.position.x;
double g_y = adjusted_global_plan[i].pose.position.y;
unsigned int map_x, map_y;
if (costmap.worldToMap(g_x, g_y, map_x, map_y) && costmap.getCost(map_x, map_y) != costmap_2d::NO_INFORMATION) {
MapCell& current = getCell(map_x, map_y);
current.target_dist = 0.0;
current.target_mark = true;
path_dist_queue.push(¤t);
started_path = true;
} else if (started_path) {
break;
}
}
if (!started_path) {
ROS_ERROR("None of the %d first of %zu (%zu) points of the global plan were in the local costmap and free",
i, adjusted_global_plan.size(), global_plan.size());
return;
}
computeTargetDistance(path_dist_queue, costmap);
}
//mark the point of the costmap as local goal where global_plan first leaves the area (or its last point)
void MapGrid::setLocalGoal(const costmap_2d::Costmap2D& costmap,//得到进行局部路径规划所需要的目标
const std::vector<geometry_msgs::PoseStamped>& global_plan) {
sizeCheck(costmap.getSizeInCellsX(), costmap.getSizeInCellsY());
int local_goal_x = -1;
int local_goal_y = -1;
bool started_path = false;
std::vector<geometry_msgs::PoseStamped> adjusted_global_plan;
adjustPlanResolution(global_plan, adjusted_global_plan, costmap.getResolution());
// skip global path points until we reach the border of the local map
for (unsigned int i = 0; i < adjusted_global_plan.size(); ++i) {//扫描在局部地图中的全局路径
double g_x = adjusted_global_plan[i].pose.position.x;
double g_y = adjusted_global_plan[i].pose.position.y;
unsigned int map_x, map_y;
if (costmap.worldToMap(g_x, g_y, map_x, map_y) && costmap.getCost(map_x, map_y) != costmap_2d::NO_INFORMATION) {
local_goal_x = map_x;
local_goal_y = map_y;
started_path = true;
} else {
if (started_path) {
break;
}// else we might have a non pruned path, so we just continue
}
}
if (!started_path) {
ROS_ERROR("None of the points of the global plan were in the local costmap, global plan points too far from robot");
return;
}
queue<MapCell*> path_dist_queue;
if (local_goal_x >= 0 && local_goal_y >= 0) {
MapCell& current = getCell(local_goal_x, local_goal_y);
costmap.mapToWorld(local_goal_x, local_goal_y, goal_x_, goal_y_);
current.target_dist = 0.0;
current.target_mark = true;
path_dist_queue.push(¤t);
}
computeTargetDistance(path_dist_queue, costmap);
}
void MapGrid::computeTargetDistance(queue<MapCell*>& dist_queue, const costmap_2d::Costmap2D& costmap){
MapCell* current_cell;
MapCell* check_cell;
unsigned int last_col = size_x_ - 1;
unsigned int last_row = size_y_ - 1;
while(!dist_queue.empty()){
current_cell = dist_queue.front();//current_cell会遍历要跟踪的路径上所有的点
dist_queue.pop();
if(current_cell->cx > 0){
check_cell = current_cell - 1;
if(!check_cell->target_mark){
//mark the cell as visisted
check_cell->target_mark = true;//对该点进行标记，证明其被visist
if(updatePathCell(current_cell, check_cell, costmap)) {//==========》重要调用获取该点的cost值，注意次函数是依据check_cell比current_cell远离路径这一技巧。
dist_queue.push(check_cell);//此处对路径的周围点进行了扩展，从而会遍历整个costmap
}
}
}
if(current_cell->cx < last_col){
check_cell = current_cell + 1;
if(!check_cell->target_mark){
check_cell->target_mark = true;
if(updatePathCell(current_cell, check_cell, costmap)) {
dist_queue.push(check_cell);//此处对路径的周围点进行了扩展，从而会遍历整个costmap
}
}
}
if(current_cell->cy > 0){
check_cell = current_cell - size_x_;
if(!check_cell->target_mark){
check_cell->target_mark = true;
if(updatePathCell(current_cell, check_cell, costmap)) {
dist_queue.push(check_cell);//此处对路径的周围点进行了扩展，从而会遍历整个costmap
}
}
}
if(current_cell->cy < last_row){
check_cell = current_cell + size_x_;
if(!check_cell->target_mark){
check_cell->target_mark = true;
if(updatePathCell(current_cell, check_cell, costmap)) {
dist_queue.push(check_cell);//此处对路径的周围点进行了扩展，从而会遍历整个costmap
}
}
}
}
}
};map_grid_cost_function.cpp
#include <base_local_planner/map_grid_cost_function.h>
namespace base_local_planner {
MapGridCostFunction::MapGridCostFunction(costmap_2d::Costmap2D* costmap,
double xshift,
double yshift,
bool is_local_goal_function,
CostAggregationType aggregationType) :
costmap_(costmap),
map_(costmap->getSizeInCellsX(), costmap->getSizeInCellsY()),// base_local_planner::MapGrid map_ = map_(costmap->getSizeInCellsX(), costmap->getSizeInCellsY());
aggregationType_(aggregationType),
xshift_(xshift),
yshift_(yshift),
is_local_goal_function_(is_local_goal_function),
stop_on_failure_(true) {}
void MapGridCostFunction::setTargetPoses(std::vector<geometry_msgs::PoseStamped> target_poses) {
target_poses_ = target_poses;
}
bool MapGridCostFunction::prepare() {//这些操作为了下面的getCellCosts函数使用
map_.resetPathDist();//对costmap的所有cell进行重置path_dist goal_dist
if (is_local_goal_function_) {
map_.setLocalGoal(*costmap_, target_poses_);
} else {
map_.setTargetCells(*costmap_, target_poses_);
}
return true;
}
double MapGridCostFunction::getCellCosts(unsigned int px, unsigned int py) {
double grid_dist = map_(px, py).target_dist;//============》距离规划出的路径的距离，此处为类对象map_的运算符重载（）。map_对象在头文件声明后，在该cpp文件进行了各种操作。类内部的成员变量map_(注意不是该map_对象)，进行了相应的初始化等操作
return grid_dist;
}
double MapGridCostFunction::scoreTrajectory(Trajectory &traj) {
double cost = 0.0;
if (aggregationType_ == Product) {
cost = 1.0;
}
double px, py, pth;
unsigned int cell_x, cell_y;
double grid_dist;
for (unsigned int i = 0; i < traj.getPointsSize(); ++i) {
traj.getPoint(i, px, py, pth);
// translate point forward if specified //某些情况下需要对轨迹上的点进行前后平移
if (xshift_ != 0.0) {
px = px + xshift_ * cos(pth);
py = py + xshift_ * sin(pth);
}
// translate point sideways if specified //某些情况下需要对轨迹上的点进行侧向平移
if (yshift_ != 0.0) {
px = px + yshift_ * cos(pth + M_PI_2);
py = py + yshift_ * sin(pth + M_PI_2);
}
//we won't allow trajectories that go off the map... shouldn't happen that often anyways
if ( ! costmap_->worldToMap(px, py, cell_x, cell_y)) {//如果进行平移后，判断轨迹是否超出了地图范围，并且获取costmap对应的点坐标
//we're off the map
ROS_WARN("Off Map %f, %f", px, py);
return -4.0;
}
grid_dist = getCellCosts(cell_x, cell_y);//=========================》获取距离规划处的路径的距离
//if a point on this trajectory has no clear path to the goal... it may be invalid
if (stop_on_failure_) {
if (grid_dist == map_.obstacleCosts()) {
return -3.0;
} else if (grid_dist == map_.unreachableCellCosts()) {
return -2.0;
}
}
switch( aggregationType_ ) {
case Last:
cost = grid_dist;
break;
case Sum:
cost += grid_dist;
break;
case Product:
if (cost > 0) {
cost *= grid_dist;
}
break;
}
}
return cost;//得到相对于跟踪路径的cost的值
}
} /* namespace base_local_planner */prefer_forward_cost_function.cpp
#include <base_local_planner/prefer_forward_cost_function.h>
#include <math.h>
namespace base_local_planner {
double PreferForwardCostFunction::scoreTrajectory(Trajectory &traj) {//机器人向后走，即扫射运动很不被鼓励，旋转速度越大越不被鼓励
// backward motions bad on a robot without backward sensors
if (traj.xv_ < 0.0) {
return penalty_;
}
// strafing motions also bad on such a robot
if (traj.xv_ < 0.1 && fabs(traj.thetav_) < 0.2) {
return penalty_;
}
// the more we rotate, the less we progress forward
return fabs(traj.thetav_) * 10;
}
} /* namespace base_local_planner */simple_scored_sampling_planner.cpp
#include <base_local_planner/simple_scored_sampling_planner.h>
#include <ros/console.h>
namespace base_local_planner {
SimpleScoredSamplingPlanner::SimpleScoredSamplingPlanner(std::vector<TrajectorySampleGenerator*> gen_list, std::vector<TrajectoryCostFunction*>& critics, int max_samples) {
max_samples_ = max_samples;
gen_list_ = gen_list;
critics_ = critics;
}
double SimpleScoredSamplingPlanner::scoreTrajectory(Trajectory& traj, double best_traj_cost) {//获取轨迹的得分
double traj_cost = 0;
int gen_id = 0;
//特别需要关注TrajectoryCostFunction类，这个对象是什么呢
//对critics_,这个对象的每次的迭代的对象不同的。
for(std::vector<TrajectoryCostFunction*>::iterator score_function = critics_.begin(); score_function != critics_.end(); ++score_function) {
TrajectoryCostFunction* score_function_p = *score_function;
if (score_function_p->getScale() == 0) {
continue;
}
//For timing uncomment
struct timeval start, end;
double start_t, end_t, t_diff;
gettimeofday(&start, NULL);
//下面这个函数调用可以计算出路径的代价。注意多态。
double cost = score_function_p->scoreTrajectory(traj);//=================》该函数下的cost
//For timing uncomment
gettimeofday(&end, NULL);
start_t = start.tv_sec + double(start.tv_usec) / 1e6;
end_t = end.tv_sec + double(end.tv_usec) / 1e6;
t_diff = end_t - start_t;
//ROS_INFO("score function time: %.9f, gen_id:%d", t_diff,gen_id);
if (cost < 0) {
ROS_DEBUG("Velocity %.3lf, %.3lf, %.3lf discarded by cost function %d with cost: %f", traj.xv_, traj.yv_, traj.thetav_, gen_id, cost);
traj_cost = cost;
break;
}
if (cost != 0) {
cost *= score_function_p->getScale();
}
//对于路径的代价需要乘以系数之后进行累加。
traj_cost += cost;
if (best_traj_cost > 0) {
// since we keep adding positives, once we are worse than the best, we will stay worse
if (traj_cost > best_traj_cost) {//如果traj_cost已经大于best_traj_cost了，且best_traj_cost，为了节省计算资源，停止计算。
break;
}
}
gen_id ++;
}
return traj_cost;
}
//get the best cost trajectory and all trajectorys we explored
//下面这个函数得到最优的轨迹，轨迹包含了速度哦。
bool SimpleScoredSamplingPlanner::findBestTrajectory(Trajectory& traj, std::vector<Trajectory>* all_explored) {//对最优轨迹和所有的轨迹都进行计算和数据填充
Trajectory loop_traj;
Trajectory best_traj;
double loop_traj_cost, best_traj_cost = -1;
bool gen_success;
int count, count_valid;
for (std::vector<TrajectoryCostFunction*>::iterator loop_critic = critics_.begin(); loop_critic != critics_.end(); ++loop_critic) {//对轨迹cost函数的准备，与检查
TrajectoryCostFunction* loop_critic_p = *loop_critic;
if (loop_critic_p->prepare() == false) {
ROS_WARN("A scoring function failed to prepare");
return false;
}
}
//we can use many generator, at this time, there is only one ,that is dwa use, we can limit the max number tra of every generator
//every generator use sample velocity space to generate many trajectories, before we use we should initialse first.
for (std::vector<TrajectorySampleGenerator*>::iterator loop_gen = gen_list_.begin(); loop_gen != gen_list_.end(); ++loop_gen) {//此处之用到了一种轨迹生成器来生成轨迹
count = 0;
count_valid = 0;
TrajectorySampleGenerator* gen_ = *loop_gen;
while (gen_->hasMoreTrajectories()) {//根据是否还有速度采样没算，检查是否还有轨迹
gen_success = gen_->nextTrajectory(loop_traj);//=========================》生成下一个轨迹。此处调用的是轨迹生成器的函数，前面的生成轨迹的依据的采样速度已经生成
if (gen_success == false) {
// TODO use this for debugging
continue;
}
loop_traj_cost = scoreTrajectory(loop_traj, best_traj_cost);//==============》对生成的轨迹进行评分
if (all_explored != NULL) {
loop_traj.cost_ = loop_traj_cost;
all_explored->push_back(loop_traj);
}
if (loop_traj_cost >= 0) {
count_valid++;
if (best_traj_cost < 0 || loop_traj_cost < best_traj_cost) {
best_traj_cost = loop_traj_cost;//将更优秀的cost赋值给best_traj_cost
best_traj = loop_traj;//将cost更低的轨迹也赋值给best_traj
}
}
count++;
if (max_samples_ > 0 && count >= max_samples_) {
break;
}
}
if (best_traj_cost >= 0) {//对最终轨迹的速度和cost进行填充
traj.xv_ = best_traj.xv_;
traj.yv_ = best_traj.yv_;
traj.thetav_ = best_traj.thetav_;
traj.cost_ = best_traj_cost;
traj.resetPoints();
double px, py, pth;
for (unsigned int i = 0; i < best_traj.getPointsSize(); i++) {
best_traj.getPoint(i, px, py, pth);
traj.addPoint(px, py, pth);//对最终轨迹的点数据进行填充
}
}
ROS_DEBUG("Evaluated %d trajectories, found %d valid", count, count_valid);
if (best_traj_cost >= 0) {
// do not try fallback generators
break;
}
}
return best_traj_cost >= 0;
}
}// namespacesimple_trajectory_generator.cpp
#include <base_local_planner/simple_trajectory_generator.h>
#include <cmath>
#include <base_local_planner/velocity_iterator.h>
namespace base_local_planner {
void SimpleTrajectoryGenerator::initialise(
const Eigen::Vector3f& pos,
const Eigen::Vector3f& vel,
const Eigen::Vector3f& goal,
base_local_planner::LocalPlannerLimits* limits,
const Eigen::Vector3f& vsamples,
std::vector<Eigen::Vector3f> additional_samples,
bool discretize_by_time) {
initialise(pos, vel, goal, limits, vsamples, discretize_by_time);
// add static samples if any
sample_params_.insert(sample_params_.end(), additional_samples.begin(), additional_samples.end());
}
//pos. the current pose of robot
//vel. the current velocity of robot
//goal, which goal???
//vsamples, the number of sampales we use in x,y,th
void SimpleTrajectoryGenerator::initialise(//下面产生采样速度
const Eigen::Vector3f& pos,
const Eigen::Vector3f& vel,
const Eigen::Vector3f& goal,
base_local_planner::LocalPlannerLimits* limits,
const Eigen::Vector3f& vsamples,
bool discretize_by_time) {//注意discretize_by_time这个参数，如果该参数为1.则生成的轨迹是以共同时间的，为0，则是以共同长度的
/*
* We actually generate all velocity sample vectors here, from which to generate trajectories later on
*/
//so the min_rot_vel is not used anyway!! min_rot_vel is used simalar as min_trans_vel
double max_vel_th = limits->max_rot_vel;
double min_vel_th = -1.0 * max_vel_th;//旋转速度是有方向的，有最大也有最小，最小为负的最大
discretize_by_time_ = discretize_by_time;
Eigen::Vector3f acc_lim = limits->getAccLimits();//三个加速度
pos_ = pos;
vel_ = vel;
limits_ = limits;
next_sample_index_ = 0;
sample_params_.clear();
double min_vel_x = limits->min_vel_x;
double max_vel_x = limits->max_vel_x;
double min_vel_y = limits->min_vel_y;
double max_vel_y = limits->max_vel_y;
// if sampling number is zero in any dimension, we don't generate samples generically//采样个数不能为0
if (vsamples[0] * vsamples[1] * vsamples[2] > 0) {
//compute the feasible velocity space based on the rate at which we run
Eigen::Vector3f max_vel = Eigen::Vector3f::Zero();
Eigen::Vector3f min_vel = Eigen::Vector3f::Zero();
if ( ! use_dwa_) {//如果use_dwa==false，首先用当前位置与目标位置的距离处理仿真时间sim_time（模拟仿真时间内匀减速到0，刚好到达目标点的情景），这样sim_time一般会明显大于sim_period
// there is no point in overshooting the goal, and it also may break the
// robot behavior, so we limit the velocities to those that do not overshoot in sim_time
double dist = hypot(goal[0] - pos[0], goal[1] - pos[1]);
max_vel_x = std::max(std::min(max_vel_x, dist / sim_time_), min_vel_x);//最大速度与在模拟时间即可到达目标所需要的速度，取最小；然后与最小速度取最大
max_vel_y = std::max(std::min(max_vel_y, dist / sim_time_), min_vel_y);
// if we use continous acceleration, we can sample the max velocity we can reach in sim_time_
max_vel[0] = std::min(max_vel_x, vel[0] + acc_lim[0] * sim_time_);//最大速度，当前速度+加速度*时间，取最小
max_vel[1] = std::min(max_vel_y, vel[1] + acc_lim[1] * sim_time_);
max_vel[2] = std::min(max_vel_th, vel[2] + acc_lim[2] * sim_time_);
min_vel[0] = std::max(min_vel_x, vel[0] - acc_lim[0] * sim_time_);//最小速度与当前速度-加速度*时间，取最大
min_vel[1] = std::max(min_vel_y, vel[1] - acc_lim[1] * sim_time_);
min_vel[2] = std::max(min_vel_th, vel[2] - acc_lim[2] * sim_time_);
} else {
// with dwa do not accelerate beyond the first step, we only sample within velocities we reach in sim_period
max_vel[0] = std::min(max_vel_x, vel[0] + acc_lim[0] * sim_period_);//只考虑sim_period
max_vel[1] = std::min(max_vel_y, vel[1] + acc_lim[1] * sim_period_);
max_vel[2] = std::min(max_vel_th, vel[2] + acc_lim[2] * sim_period_);
min_vel[0] = std::max(min_vel_x, vel[0] - acc_lim[0] * sim_period_);
min_vel[1] = std::max(min_vel_y, vel[1] - acc_lim[1] * sim_period_);
min_vel[2] = std::max(min_vel_th, vel[2] - acc_lim[2] * sim_period_);
}
Eigen::Vector3f vel_samp = Eigen::Vector3f::Zero();//三维采样速度
VelocityIterator x_it(min_vel[0], max_vel[0], vsamples[0]);
VelocityIterator y_it(min_vel[1], max_vel[1], vsamples[1]);
VelocityIterator th_it(min_vel[2], max_vel[2], vsamples[2]);
for(; !x_it.isFinished(); x_it++) {
vel_samp[0] = x_it.getVelocity();
for(; !y_it.isFinished(); y_it++) {
vel_samp[1] = y_it.getVelocity();
for(; !th_it.isFinished(); th_it++) {
vel_samp[2] = th_it.getVelocity();
//ROS_DEBUG("Sample %f, %f, %f", vel_samp[0], vel_samp[1], vel_samp[2]);
sample_params_.push_back(vel_samp);//所有的采样速度都存在了sample_params
}
th_it.reset();
}
y_it.reset();
}
}
}
void SimpleTrajectoryGenerator::setParameters(
double sim_time,
double sim_granularity,
double angular_sim_granularity,
bool use_dwa,
double sim_period) {
sim_time_ = sim_time;
sim_granularity_ = sim_granularity;
angular_sim_granularity_ = angular_sim_granularity;
use_dwa_ = use_dwa;
continued_acceleration_ = ! use_dwa_;
sim_period_ = sim_period;
}
/**
* Whether this generator can create more trajectories
*/
bool SimpleTrajectoryGenerator::hasMoreTrajectories() {
return next_sample_index_ < sample_params_.size();
}
/**
* Create and return the next sample trajectory
*/
bool SimpleTrajectoryGenerator::nextTrajectory(Trajectory &comp_traj) {//产生下一个采样轨迹
bool result = false;
if (hasMoreTrajectories()) {
if (generateTrajectory(//====================》该函数为根据采样速度生成采样轨迹的函数
pos_,
vel_,
sample_params_[next_sample_index_],//生成的采样速度等信息
comp_traj)) {
result = true;
}
}
next_sample_index_++;
return result;
}
/**
* @param pos current position of robot
* @param vel current velocity of robot
* @param sample_target_vel desired velocity for sampling
*/
bool SimpleTrajectoryGenerator::generateTrajectory(
Eigen::Vector3f pos,//当前位置
Eigen::Vector3f vel,//当前速度
Eigen::Vector3f sample_target_vel,//产生的采样速度
base_local_planner::Trajectory& traj) {//生成的轨迹
double vmag = hypot(sample_target_vel[0], sample_target_vel[1]);//合成的速度大小
double eps = 1e-4;
traj.cost_ = -1.0; // placed here in case we return early//防止每更新即返回，所以将轨迹设置为不可用
//trajectory might be reused so we'll make sure to reset it
traj.resetPoints();//轨迹重置
// make sure that the robot would at least be moving with one of
// the required minimum velocities for translation and rotation (if set)
if ((limits_->min_trans_vel >= 0 && vmag + eps < limits_->min_trans_vel) &&
(limits_->min_rot_vel >= 0 && fabs(sample_target_vel[2]) + eps < limits_->min_rot_vel)) {//确保机器人是可以运动的，满足运动条件
return false;
}
// make sure we do not exceed max diagonal (x+y) translational velocity (if set)
if (limits_->max_trans_vel >=0 && vmag - eps > limits_->max_trans_vel) {//确保合成速度不大于设定的最大合成速度
return false;
}
int num_steps;
if (discretize_by_time_) {//使用全程的模拟时间（--上面生成速度时亦是使用了该时间），以此确定积分步数
num_steps = ceil(sim_time_ / sim_granularity_);
} else {
//compute the number of steps we must take along this trajectory to be "safe"
double sim_time_distance = vmag * sim_time_; // the distance the robot would travel in sim_time if it did not change velocity
double sim_time_angle = fabs(sample_target_vel[2]) * sim_time_; // the angle the robot would rotate in sim_time
num_steps =
ceil(std::max(sim_time_distance / sim_granularity_,
sim_time_angle / angular_sim_granularity_));//如果没用discretize_by_time_，求积分步数num_steps
}
//compute a timestep
double dt = sim_time_ / num_steps;
traj.time_delta_ = dt;
Eigen::Vector3f loop_vel;
if (continued_acceleration_) {//对loop_vel进行初始化
// assuming the velocity of the first cycle is the one we want to store in the trajectory object
loop_vel = computeNewVelocities(sample_target_vel, vel, limits_->getAccLimits(), dt);
traj.xv_ = loop_vel[0];
traj.yv_ = loop_vel[1];
traj.thetav_ = loop_vel[2];
} else {
// assuming sample_vel is our target velocity within acc limits for one timestep
loop_vel = sample_target_vel;
traj.xv_ = sample_target_vel[0];
traj.yv_ = sample_target_vel[1];
traj.thetav_ = sample_target_vel[2];
}
//simulate the trajectory and check for collisions, updating costs along the way
for (int i = 0; i < num_steps; ++i) {
//add the point to the trajectory so we can draw it later if we want
traj.addPoint(pos[0], pos[1], pos[2]);//对轨迹的位置点的位姿进行添加
if (continued_acceleration_) {
//calculate velocities
loop_vel = computeNewVelocities(sample_target_vel, loop_vel, limits_->getAccLimits(), dt);
//ROS_WARN_NAMED("Generator", "Flag: %d, Loop_Vel %f, %f, %f", continued_acceleration_, loop_vel[0], loop_vel[1], loop_vel[2]);
}
//update the position of the robot using the velocities passed in
pos = computeNewPositions(pos, loop_vel, dt);//计算模拟轨迹的下一个点的位姿
} // end for simulation steps
return num_steps > 0; // true if trajectory has at least one point
}
Eigen::Vector3f SimpleTrajectoryGenerator::computeNewPositions(const Eigen::Vector3f& pos,//计算轨迹下一个点的位姿
const Eigen::Vector3f& vel, double dt) {
Eigen::Vector3f new_pos = Eigen::Vector3f::Zero();
new_pos[0] = pos[0] + (vel[0] * cos(pos[2]) + vel[1] * cos(M_PI_2 + pos[2])) * dt;
new_pos[1] = pos[1] + (vel[0] * sin(pos[2]) + vel[1] * sin(M_PI_2 + pos[2])) * dt;
new_pos[2] = pos[2] + vel[2] * dt;
return new_pos;
}
/**
* cheange vel using acceleration limits to converge towards sample_target-vel
*/
Eigen::Vector3f SimpleTrajectoryGenerator::computeNewVelocities(const Eigen::Vector3f& sample_target_vel,//根据dt，和加速度限制。积分式的加减速度
const Eigen::Vector3f& vel, Eigen::Vector3f acclimits, double dt) {
Eigen::Vector3f new_vel = Eigen::Vector3f::Zero();
for (int i = 0; i < 3; ++i) {
if (vel[i] < sample_target_vel[i]) {
new_vel[i] = std::min(double(sample_target_vel[i]), vel[i] + acclimits[i] * dt);
} else {
new_vel[i] = std::max(double(sample_target_vel[i]), vel[i] - acclimits[i] * dt);
}
}
return new_vel;
}
} /* namespace base_local_planner */
3384
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
