//------------------------------------------------------------------------------
/// 値の表す座標 (x, y) と引数 aPoint の表す座標間の距離を返します。
///
/// @param[in] aPoint 距離計算の目的地となる座標。
///
/// @return (x, y) と aPoint の距離。
float Vec2::dist(const Vec2& aPoint)const
{
return Math::Sqrt(squareDist(aPoint));
}
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;
}
// Calculating distance bewteen 2 particles
float dist(struct Particle *a, struct Particle *b){
return sqrt(squareDist(a, b));
}
float
dist(const Vector<N, T>& v1, const Vector<N, T>& v2)
{
return std::sqrt(squareDist(v1, v2));
}
