概要
ここではローカルプランナーについて分析する。
前提条件
- ROS distroはmelodic
- move_baseソースコード:https://github.com/ros-planning/navigation/tree/melodic-devel
- ソースコードから解析した内容を記す。
- dwa_local_plannerのみを対象とする。
- base_local_plannerに準拠したプランナーなら、大まかな処理の流れは同じはず。
参考
- 産総研によるnavigationスタックの解説資料です。この記事を作った後に気づきました(汗)。
dwa_local_plannerについて
- DWA(Dynamic Window Approch)をローカルプランナーに採用した手法？
- 設定でdwaを使わないこともできる。
DWA(Dynamic Window Approch)の基本知識
- 以下のサイトに素晴らしい説明が載っているので参照。サンプルコードもあり、純粋なDWAの実装を確認できる。
- https://myenigma.hatenablog.com/entry/20140624/1403618922
- dwa_local_plannerは上記の純粋なDWAからは色々変更が入っているが、基本的な考えは同じ。
- 重要な知識
- DW(ダイナミックウィンドウ）
- コスト関数
関連クラス
- DWAPlanner
- dwaを使用してローカルプランナーを実装するクラス
- DWAPlannerROS
- DWAPlannerをROSから使えるようにしたラッパークラス
- 軌道生成
- SimpleTrajectoryGenerator
- dwaの原理を利用して軌道を生成するクラス
- SimpleTrajectoryGenerator
- コスト関数
- OscillationCostFunction
- 振動による影響を軽減するために評価するクラス
- ObstacleCostFunction
- 障害物と衝突する可能性を評価するクラス
- MapGridCostFunction
- 軌道がグローバルパスを辿るか、ゴールにどれだけ近づくかを評価するクラス。
- TwirlingCostFunction
- ロボットがゴールに向かう途中での回転量に基づいてコストを評価するクラス。
- OscillationCostFunction
- SimpleScoredSamplingPlanner
- SimpleTrajectoryGeneratorで生成された軌道を取得し設定されているコスト関数で評価しその合計を返すクラス。
- dwa_local_plannerは4つのコスト関数が設定されているので、1つの軌道に対して4つのコスト関数を適用する
設定可能なパラメータ
- 以前記事を書いたのでそれを参照。
ローカルプランナーの簡易フロー
- 初期化
- move_baseでローカルプランナーの初期化処理
- ローカルプランナーにグローバルプランをセット
- メイン処理
- 速度コマンド計算処理呼び出し
- 最良の軌道を探索
- ジェネレーターを準備
- サンプリングした値で軌道を評価
詳細
初期化処理
- 以下の処理で指定したローカルプランナーの初期化処理を行っている。
move_base.cpp
// create a local planner
try
{
// テンプレートクラスClassLoaderを用いてローカルプランナーのインスタンスを作成
tc_ = blp_loader_.createInstance(local_planner);
ROS_INFO("Created local_planner %s", local_planner.c_str());
tc_->initialize(blp_loader_.getName(local_planner), &tf_, controller_costmap_ros_);
}
- 初期化処理で呼び出されるものは以下の順で呼び出される。
- DWAPlannerROSクラスのinitialize処理
- DWAPlannerのコンストラクタ
- メンバ変数
- critics_はコスト関数と係数を保持するメンバ
- generator_はdwを用いた軌道の取りうる範囲を保持するメンバ
- scored_sampling_planner_はgenerator_とcritics_を受け取って軌道を評価するメンバ
- メンバ変数
dwa_planner.cpp
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())
, goal_costs_(planner_util->getCostmap(), 0.0, 0.0, true)
, goal_front_costs_(planner_util->getCostmap(), 0.0, 0.0, true)
, alignment_costs_(planner_util->getCostmap())
{
...
// 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;
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(&goal_front_costs_); // prefers trajectories that make the
// nose go towards (local) nose goal
critics.push_back(&alignment_costs_); // prefers trajectories that keep the
// robot nose on nose path
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
critics.push_back(&twirling_costs_); // optionally prefer trajectories that don't spin
// trajectory generators
std::vector<base_local_planner::TrajectorySampleGenerator *> generator_list;
generator_list.push_back(&generator_);
// dwを格納するジェネレーターとコスト関数をまとめた配列を軌道を評価するインスタンスに渡す。
scored_sampling_planner_ = base_local_planner::SimpleScoredSamplingPlanner(generator_list, critics);
...
}
ローカルプランナーにグローバルプランをセット
- move_base.cppのexecuteCycle()関数でグローバルプランの生成をチェックし、生成されていればローカルプランナーにセットする。
move_base.cpp
// if we have a new plan then grab it and give it to the controller
if (new_global_plan_)
{
...
if (!tc_->setPlan(*controller_plan_))// ローカルプランにグローバルプランをセット
{
// ローカルプランナーに計画をセットできない場合、異常判定
// ABORT and SHUTDOWN COSTMAPS
ROS_ERROR("Failed to pass global plan to the controller, aborting.");
resetState();
// disable the planner thread
lock.lock();
runPlanner_ = false;
lock.unlock();
as_->setAborted(move_base_msgs::MoveBaseResult(), "Failed to pass global plan to the controller.");
return true;
}
...
}
速度コマンド計算処理呼び出し
-
computeVelocityCommands()がdwa_local_plannerのメイン処理を呼び出すためにラッパークラスを呼び出す関数。dwa_local_planner以外でも、base_local_plannerに準拠したローカルプランナーをmove_baseから呼び出す場合、ラッパークラスのこの関数を経由する。
move_base.cpp
if (tc_->computeVelocityCommands(cmd_vel))
{
ROS_DEBUG_NAMED("move_base", "Got a valid command from the local planner: %.3lf, %.3lf, %.3lf",
cmd_vel.linear.x, cmd_vel.linear.y, cmd_vel.angular.z);
last_valid_control_ = ros::Time::now();
// make sure that we send the velocity command to the base
vel_pub_.publish(cmd_vel);
if (recovery_trigger_ == CONTROLLING_R)
recovery_index_ = 0;
}
-
computeVelocityCommands()内は以下のような処理構成になっている。- はじめにコスト関数にグローバルプランを渡して更新する:
dp_->updatePlanAndLocalCosts(current_pose_, transformed_plan, costmap_ros_->getRobotFootprint());- dwa_local_plannerはMapGridCostFunctionクラス以外のコスト関数はグローバルプランを取らない。
- 次にまだゴールしていない場合、ラッパークラスの
dwaComputeVelocityCommands()を呼び出す。
- はじめにコスト関数にグローバルプランを渡して更新する:
dwa_planner_ros.cpp
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, costmap_ros_->getRobotFootprint());
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);
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;
}
}
- この中で呼び出されている
findBestPath()がdwa_local_plannerのメインとなる処理。 - 最良の軌道が得られた場合、その軌道の並進速度と旋回速度を受け取る。
dwa_planner_ros.cpp
bool DWAPlannerROS::dwaComputeVelocityCommands(geometry_msgs::PoseStamped &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;
}
geometry_msgs::PoseStamped 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
geometry_msgs::PoseStamped drive_cmds;
drive_cmds.header.frame_id = costmap_ros_->getBaseFrameID();
// call with updated footprint
// 最良の軌道を取得
base_local_planner::Trajectory path = dp_->findBestPath(global_pose, robot_vel, drive_cmds);
// 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_INFO("Cycle time: %.9f", t_diff);
*/
// pass along drive commands
cmd_vel.linear.x = drive_cmds.pose.position.x;
cmd_vel.linear.y = drive_cmds.pose.position.y;
cmd_vel.angular.z = tf2::getYaw(drive_cmds.pose.orientation);
// 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);
geometry_msgs::PoseStamped p;
p.header.frame_id = costmap_ros_->getGlobalFrameID();
p.header.stamp = ros::Time::now();
p.pose.position.x = p_x;
p.pose.position.y = p_y;
p.pose.position.z = 0.0;
tf2::Quaternion q;
q.setRPY(0, 0, p_th);
tf2::convert(q, p.pose.orientation);
local_plan.push_back(p);
}
// publish information to the visualizer
publishLocalPlan(local_plan);
return true;
}
最良の軌道を探索
-
findBestPath()関数の主な処理は2つ。- dwaによる軌道を算出する範囲を作成:
generator_.initialise(pos, vel, goal, &limits, vsamples_) - サンプリングした値で最良の軌道を探索:
scored_sampling_planner_.findBestTrajectory(result_traj_, &all_explored);
- dwaによる軌道を算出する範囲を作成:
dwa_planner.cpp
/*
* given the current state of the robot, find a good trajectory
*/
base_local_planner::Trajectory DWAPlanner::findBestPath(const geometry_msgs::PoseStamped &global_pose,
const geometry_msgs::PoseStamped &global_vel,
geometry_msgs::PoseStamped &drive_velocities)
{
...
// prepare cost functions and generators for this run
generator_.initialise(pos, vel, goal, &limits, vsamples_);
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);
...
// debrief stateful scoring functions
oscillation_costs_.updateOscillationFlags(pos, &result_traj_, planner_util_->getCurrentLimits().min_vel_trans);
// if we don't have a legal trajectory, we'll just command zero
if (result_traj_.cost_ < 0)
{
// 有効な軌道出ない場合、速度0のコマンド
drive_velocities.pose.position.x = 0;
drive_velocities.pose.position.y = 0;
drive_velocities.pose.position.z = 0;
drive_velocities.pose.orientation.w = 1;
drive_velocities.pose.orientation.x = 0;
drive_velocities.pose.orientation.y = 0;
drive_velocities.pose.orientation.z = 0;
}
else
{
drive_velocities.pose.position.x = result_traj_.xv_;
drive_velocities.pose.position.y = result_traj_.yv_;
drive_velocities.pose.position.z = 0;
tf2::Quaternion q;
q.setRPY(0, 0, result_traj_.thetav_);
tf2::convert(q, drive_velocities.pose.orientation);
}
return result_traj_;
}
ジェネレーターを準備
- ここでは、sample_params_にdwで算出した範囲の値をvsamplesで均等にサンプリングして[x_vel,v_vel,theta_vel]の組として格納する。
simple_trajectory_generator.cpp
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_vel_theta;
double min_vel_th = -1.0 * max_vel_th;
discretize_by_time_ = discretize_by_time; // defaultはfalse
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
// vsamples[0]->x軸方向のサンプル数、vsamples[1]->y軸方向のサンプル数、vsamples[2]->theta方向のサンプル数
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
// dwを算出
// パラメータmax_vel_xと(現在のx軸方向の速度 + 加速度の上限 * sim_period_(デフォルトなら0.05))
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();
// min_vel[0]からmax_vel[0]までの値を均等にvsamples[0]個になるようにする。
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]);
// サンプリングされた組をvel_sampleを格納していく。
sample_params_.push_back(vel_samp);
}
th_it.reset();
}
y_it.reset();
}
}
}
サンプリングした値で軌道を評価
- ここでやっていることは以下の通り
- ジェネレータのサンプル数がなくなるか、max_samplesに達するまでジェネレータを呼び出し軌道を算出する。:
gen_->nextTrajectory(loop_traj) - 算出した軌道に対しコスト関数で総コストを算出 :
loop_traj_cost = scoreTrajectory(loop_traj, best_traj_cost) - best_traj_costより小さく、なおかつ0以上のloop_traj_costが最良の軌道となる。
- ジェネレータのサンプル数がなくなるか、max_samplesに達するまでジェネレータを呼び出し軌道を算出する。:
simple_scored_sampling_planner.cpp
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;
/*
std::vector<TrajectorySampleGenerator*> gen_list_;
std::vector<TrajectoryCostFunction*> critics_;
*/
for (std::vector<TrajectoryCostFunction *>::iterator loop_critic = critics_.begin(); loop_critic != critics_.end();
++loop_critic)
{
// 準備が完了しているかチェックする。
TrajectoryCostFunction *loop_critic_p = *loop_critic;
if (loop_critic_p->prepare() == false)
{
ROS_WARN("A scoring function failed to prepare");
return false;
}
}
// 複数のジェネレータを用意していれば複数回ループするが基本は1回だと思う。
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;
best_traj = loop_traj;
}
}
count++;
if (max_samples_ > 0 && count >= max_samples_)
{
// 最大サンプリング数に達したら終了
break;
}
}
if (best_traj_cost >= 0)
{
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;
}
- 軌道の評価は以下のように行う。
- 軌道を評価 :
double cost = score_function_p->scoreTrajectory(traj)- デフォルトでは7個のコスト関数が設定されているので総コストは以下の評価関数で求めている。
- 評価関数：
a×x1+b×x2+c×x3+d×x4+e×x5+f×x6+g×x7- a :
1.0(インスタンス生成時の初期値: x1が0か-5.0しか取らないため) - x1:
oscillation_costs_.scoreTrajectory() - b :
0.01(デフォルト設定: occdist_scale) - x2:
obstalce_costs_.scoreTrajectory() - c :
0.08(デフォルト設定: コストマップの解像度×goal_distance_bias=0.1×0.8) - x3:
goal_front_costs_.scoreTrajectory() - d :
0.06(デフォルト設定: コストマップの解像度×path_distance_bias=0.1×0.6) - x4:
alignments_costs_.scoreTrajectory() - e :
0.06(デフォルト設定: コストマップの解像度×path_distance_bias=0.1×0.6) - x5:
path_costs_.scoreTrajectory() - f :
0.08(デフォルト設定: コストマップの解像度×goal_distance_bias=0.1×0.8) - x6:
goal_costs_.scoreTrajectory() - g :
0(デフォルト設定: twirling_scale) - x7:
twirling_costs_.scoreTrajectory()
- a :
- コストが負の場合、その時点で他のコスト関数による評価はスキップ
- コストが0以外の場合、各コスト関数に設定した係数をコストに掛ける。
- コストが非負の場合、コストの合計を算出する。
- best_traj_cost > 0のとき、現在計算中のコスト値が上回ったら終了。
- best_traj_cost =< 0のとき、コストの合計算出を続ける。
- 軌道を評価 :
simple_scored_sampling_planner.cpp
double SimpleScoredSamplingPlanner::scoreTrajectory(Trajectory &traj, double best_traj_cost)
{
double traj_cost = 0;
int gen_id = 0;
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)
{
// 係数が0の場合は現在のコスト関数の処理をスキップする。
continue;
}
// コスト関数の数だけ実行して各コストを算出。
double cost = score_function_p->scoreTrajectory(traj);
if (cost < 0)
{
//軌道のコストに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)
{
break;
}
}
gen_id++;
}
return traj_cost;