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) {
/*
* We actually generate all velocity sample vectors here, from which to generate trajectories later on
*/
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
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_) {
// 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_);
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);
}
th_it.reset();
}
y_it.reset();
}
}
}
base_local_planner::Trajectory DWAPlanner::computeTrajectories(const Eigen::Vector3f& pos, const Eigen::Vector3f& vel){
tf::Stamped<tf::Pose> robot_pose_tf;
geometry_msgs::PoseStamped robot_pose;
//compute the distance between the robot and the last point on the global_plan
costmap_ros_->getRobotPose(robot_pose_tf);
tf::poseStampedTFToMsg(robot_pose_tf, robot_pose);
double sq_dist = squareDist(robot_pose, global_plan_.back());
bool two_point_scoring = true;
if(sq_dist < forward_point_distance_ * forward_point_distance_)
ROS_INFO("single points scoring");
two_point_scoring = false;
//compute the feasible velocity space based on the rate at which we run
Eigen::Vector3f max_vel = Eigen::Vector3f::Zero();
max_vel[0] = std::min(max_vel_x_, vel[0] + acc_lim_[0] * 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_);
Eigen::Vector3f min_vel = Eigen::Vector3f::Zero();
min_vel[0] = std::max(min_vel_x_, vel[0] - dec_lim_[0] * sim_period_);
min_vel[1] = std::max(min_vel_y_, vel[1] - dec_lim_[1] * sim_period_);
min_vel[2] = std::max(min_vel_th_, vel[2] - dec_lim_[2] * sim_period_);
Eigen::Vector3f dv = Eigen::Vector3f::Zero();
//we want to sample the velocity space regularly
for(unsigned int i = 0; i < 3; ++i){
dv[i] = (max_vel[i] - min_vel[i]) / (std::max(1.0, double(vsamples_[i]) - 1));
}
//keep track of the best trajectory seen so far... we'll re-use two memeber vars for efficiency
base_local_planner::Trajectory* best_traj = &traj_one_;
best_traj->cost_ = -1.0;
base_local_planner::Trajectory* comp_traj = &traj_two_;
comp_traj->cost_ = -1.0;
Eigen::Vector3f vel_samp = Eigen::Vector3f::Zero();
//ROS_ERROR("x(%.2f, %.2f), y(%.2f, %.2f), th(%.2f, %.2f)", min_vel[0], max_vel[0], min_vel[1], max_vel[1], min_vel[2], max_vel[2]);
//ROS_ERROR("x(%.2f, %.2f), y(%.2f, %.2f), th(%.2f, %.2f)", min_vel_x_, max_vel_x_, min_vel_y_, max_vel_y_, min_vel_th_, max_vel_th_);
//ROS_ERROR("dv %.2f %.2f %.2f", dv[0], dv[1], dv[2]);
for(VelocityIterator x_it(min_vel[0], max_vel[0], dv[0]); !x_it.isFinished(); x_it++){
vel_samp[0] = x_it.getVelocity();
for(VelocityIterator y_it(min_vel[1], max_vel[1], dv[1]); !y_it.isFinished(); y_it++){
vel_samp[1] = y_it.getVelocity();
for(VelocityIterator th_it(min_vel[2], max_vel[2], dv[2]); !th_it.isFinished(); th_it++){
vel_samp[2] = th_it.getVelocity();
generateTrajectory(pos, vel_samp, *comp_traj, two_point_scoring, sq_dist);
selectBestTrajectory(best_traj, comp_traj);
}
}
}
ROS_DEBUG_NAMED("oscillation_flags", "forward_pos_only: %d, forward_neg_only: %d, strafe_pos_only: %d, strafe_neg_only: %d, rot_pos_only: %d, rot_neg_only: %d",
forward_pos_only_, forward_neg_only_, strafe_pos_only_, strafe_neg_only_, rot_pos_only_, rot_neg_only_);
//ok... now we have our best trajectory
if(best_traj->cost_ >= 0){
//we want to check if we need to set any oscillation flags
if(setOscillationFlags(best_traj)){
prev_stationary_pos_ = pos;
}
//if we've got restrictions... check if we can reset any oscillation flags
if(forward_pos_only_ || forward_neg_only_
|| strafe_pos_only_ || strafe_neg_only_
|| rot_pos_only_ || rot_neg_only_){
resetOscillationFlagsIfPossible(pos, prev_stationary_pos_);
}
}
//TODO: Think about whether we want to try to do things like back up when a valid trajectory is not found
return *best_traj;
}
