//------------------------------------------------------------------------------ /// 値の表す座標 (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)); }