goal_critic.cpp

// Copyright (c) 2022 Samsung Research America, @artofnothingness Alexey Budyakov
// Copyright (c) 2023 Open Navigation LLC
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//     http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
 
#include "nav2_mppi_controller/critics/goal_critic.hpp"
 
namespace mppi::critics
{
 
using xt::evaluation_strategy::immediate;
 
void GoalCritic::initialize()
{
  auto getParam = parameters_handler_->getParamGetter(name_);
 
  getParam(power_, "cost_power", 1);
  getParam(weight_, "cost_weight", 5.0f);
  getParam(threshold_to_consider_, "threshold_to_consider", 1.4f);
 
  RCLCPP_INFO(
    logger_, "GoalCritic instantiated with %d power and %f weight.",
    power_, weight_);
}
 
void GoalCritic::score(CriticData & data)
{
  if (!enabled_ || !utils::withinPositionGoalTolerance(
      threshold_to_consider_, data.state.pose.pose, data.goal))
  {
    return;
  }
 
  const auto & goal_x = data.goal.position.x;
  const auto & goal_y = data.goal.position.y;
 
  const auto traj_x = xt::view(data.trajectories.x, xt::all(), xt::all());
  const auto traj_y = xt::view(data.trajectories.y, xt::all(), xt::all());
 
  if (power_ > 1u) {
    data.costs += xt::pow(
      xt::mean(
        xt::hypot(traj_x - goal_x, traj_y - goal_y),
        {1}, immediate) * weight_, power_);
  } else {
    data.costs += xt::mean(
      xt::hypot(traj_x - goal_x, traj_y - goal_y),
      {1}, immediate) * weight_;
  }
}
 
}  // namespace mppi::critics
 
#include <pluginlib/class_list_macros.hpp>
 
PLUGINLIB_EXPORT_CLASS(mppi::critics::GoalCritic, mppi::critics::CriticFunction)