nav2_smoother.cpp

// Copyright (c) 2019 RoboTech Vision
// Copyright (c) 2019 Intel Corporation
// Copyright (c) 2022 Samsung Research America
//
// 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 <chrono>
#include <memory>
#include <string>
#include <utility>
#include <vector>
 
#include "nav2_core/smoother_exceptions.hpp"
#include "nav2_smoother/nav2_smoother.hpp"
#include "nav2_ros_common/node_utils.hpp"
#include "nav2_ros_common/tf2_factories.hpp"
 
using namespace std::chrono_literals;
 
namespace nav2_smoother
{
 
SmootherServer::SmootherServer(const rclcpp::NodeOptions & options)
: LifecycleNode("smoother_server", "", options),
  lp_loader_("nav2_core", "nav2_core::Smoother"),
  default_ids_{"simple_smoother"},
  default_types_{"nav2_smoother::SimpleSmoother"}
{
  RCLCPP_INFO(get_logger(), "Creating smoother server");
}
 
SmootherServer::~SmootherServer()
{
  smoothers_.clear();
}
 
nav2::CallbackReturn
SmootherServer::on_configure(const rclcpp_lifecycle::State & state)
{
  RCLCPP_INFO(get_logger(), "Configuring smoother server");
 
  auto node = shared_from_this();
 
  std::string costmap_topic, footprint_topic, robot_base_frame;
  double transform_tolerance = 0.1;
  costmap_topic = node->declare_or_get_parameter(
    "costmap_topic", std::string("global_costmap/costmap_raw"));
  footprint_topic = node->declare_or_get_parameter(
    "footprint_topic", std::string("global_costmap/published_footprint"));
  robot_base_frame = node->declare_or_get_parameter(
    "robot_base_frame", std::string("base_link"));
  transform_tolerance = node->declare_or_get_parameter("transform_tolerance", 0.1);
  smoother_ids_ = node->declare_or_get_parameter("smoother_plugins", default_ids_);
 
  if (smoother_ids_ == default_ids_) {
    for (size_t i = 0; i < default_ids_.size(); ++i) {
      nav2::declare_parameter_if_not_declared(
        node, default_ids_[i] + ".plugin",
        rclcpp::ParameterValue(default_types_[i]));
    }
  }
 
  tf_ = nav2::create_transform_buffer(this);
  transform_listener_ = nav2::create_transform_listener(*tf_, this, true);
 
  costmap_sub_ = std::make_shared<nav2_costmap_2d::CostmapSubscriber>(
    shared_from_this(), costmap_topic);
  footprint_sub_ = std::make_shared<nav2_costmap_2d::FootprintSubscriber>(
    shared_from_this(), footprint_topic, *tf_, robot_base_frame, transform_tolerance);
 
  collision_checker_ =
    std::make_shared<nav2_costmap_2d::CostmapTopicCollisionChecker>(
    *costmap_sub_, *footprint_sub_, this->get_name());
 
  if (!loadSmootherPlugins()) {
    on_cleanup(state);
    return nav2::CallbackReturn::FAILURE;
  }
 
  // Initialize pubs & subs
  plan_publisher_ = create_publisher<nav_msgs::msg::Path>("plan_smoothed");
 
  // Create the action server that we implement with our smoothPath method
  action_server_ = create_action_server<Action>(
    "smooth_path",
    std::bind(&SmootherServer::smoothPlan, this),
    std::bind(&SmootherServer::goalReceived, this, std::placeholders::_1),
    nullptr,
    std::chrono::milliseconds(500),
    true);
 
  return nav2::CallbackReturn::SUCCESS;
}
 
bool SmootherServer::loadSmootherPlugins()
{
  auto node = shared_from_this();
 
  smoother_types_.resize(smoother_ids_.size());
 
  for (size_t i = 0; i != smoother_ids_.size(); i++) {
    try {
      smoother_types_[i] =
        nav2::get_plugin_type_param(node, smoother_ids_[i]);
      nav2_core::Smoother::Ptr smoother =
        lp_loader_.createUniqueInstance(smoother_types_[i]);
      RCLCPP_INFO(
        get_logger(), "Created smoother : %s of type %s",
        smoother_ids_[i].c_str(), smoother_types_[i].c_str());
      smoother->configure(
        node, smoother_ids_[i], tf_, costmap_sub_,
        footprint_sub_);
      smoothers_.insert({smoother_ids_[i], smoother});
    } catch (const std::exception & ex) {
      RCLCPP_FATAL(
        get_logger(), "Failed to create smoother. Exception: %s",
        ex.what());
      return false;
    }
  }
 
  for (size_t i = 0; i != smoother_ids_.size(); i++) {
    smoother_ids_concat_ += smoother_ids_[i] + std::string(" ");
  }
 
  RCLCPP_INFO(
    get_logger(), "Smoother Server has %s smoothers available.",
    smoother_ids_concat_.c_str());
 
  return true;
}
 
nav2::CallbackReturn
SmootherServer::on_activate(const rclcpp_lifecycle::State & /*state*/)
{
  RCLCPP_INFO(get_logger(), "Activating");
 
  plan_publisher_->on_activate();
  SmootherMap::iterator it;
  for (it = smoothers_.begin(); it != smoothers_.end(); ++it) {
    it->second->activate();
  }
  action_server_->activate();
 
  // create bond connection
  createBond();
 
  return nav2::CallbackReturn::SUCCESS;
}
 
nav2::CallbackReturn
SmootherServer::on_deactivate(const rclcpp_lifecycle::State &)
{
  RCLCPP_INFO(get_logger(), "Deactivating");
 
  action_server_->deactivate();
  SmootherMap::iterator it;
  for (it = smoothers_.begin(); it != smoothers_.end(); ++it) {
    it->second->deactivate();
  }
  plan_publisher_->on_deactivate();
 
  // destroy bond connection
  destroyBond();
 
  return nav2::CallbackReturn::SUCCESS;
}
 
nav2::CallbackReturn
SmootherServer::on_cleanup(const rclcpp_lifecycle::State &)
{
  RCLCPP_INFO(get_logger(), "Cleaning up");
 
  // Cleanup the helper classes
  SmootherMap::iterator it;
  for (it = smoothers_.begin(); it != smoothers_.end(); ++it) {
    it->second->cleanup();
  }
  smoothers_.clear();
 
  // Release any allocated resources
  action_server_.reset();
  plan_publisher_.reset();
  transform_listener_.reset();
  tf_.reset();
  footprint_sub_.reset();
  costmap_sub_.reset();
  collision_checker_.reset();
 
  return nav2::CallbackReturn::SUCCESS;
}
 
nav2::CallbackReturn
SmootherServer::on_shutdown(const rclcpp_lifecycle::State &)
{
  RCLCPP_INFO(get_logger(), "Shutting down");
  return nav2::CallbackReturn::SUCCESS;
}
 
bool SmootherServer::findSmootherId(
  const std::string & c_name,
  std::string & current_smoother)
{
  if (smoothers_.find(c_name) == smoothers_.end()) {
    if (smoothers_.size() == 1 && c_name.empty()) {
      RCLCPP_WARN_ONCE(
        get_logger(),
        "No smoother was specified in action call."
        " Server will use only plugin loaded %s. "
        "This warning will appear once.",
        smoother_ids_concat_.c_str());
      current_smoother = smoothers_.begin()->first;
    } else {
      RCLCPP_ERROR(
        get_logger(),
        "SmoothPath called with smoother name %s, "
        "which does not exist. Available smoothers are: %s.",
        c_name.c_str(), smoother_ids_concat_.c_str());
      return false;
    }
  } else {
    RCLCPP_DEBUG(get_logger(), "Selected smoother: %s.", c_name.c_str());
    current_smoother = c_name;
  }
 
  return true;
}
 
bool SmootherServer::goalReceived(std::shared_ptr<const Action::Goal> goal)
{
  std::string current_smoother;
  if (!findSmootherId(goal->smoother_id, current_smoother)) {
    RCLCPP_WARN(
      get_logger(),
      "Requested smoother %s is not available.", goal->smoother_id.c_str());
    return false;
  }
  if (!validate(goal->path)) {
    RCLCPP_WARN(get_logger(), "Requested path to smooth is invalid.");
    return false;
  }
  return true;
}
 
void SmootherServer::smoothPlan()
{
  auto start_time = this->now();
 
  RCLCPP_INFO(get_logger(), "Received a path to smooth.");
 
  auto result = std::make_shared<Action::Result>();
  try {
    auto goal = action_server_->get_current_goal();
    if (!goal) {
      return;  //  if action_server_ is deactivate, goal would be a nullptr
    }
 
    std::string current_smoother;
    findSmootherId(goal->smoother_id, current_smoother);
    current_smoother_ = current_smoother;
 
    // Perform smoothing
    result->path = goal->path;
 
    result->was_completed = smoothers_[current_smoother_]->smooth(
      result->path, goal->max_smoothing_duration);
    result->smoothing_duration = this->now() - start_time;
 
    if (!result->was_completed) {
      RCLCPP_INFO(
        get_logger(),
        "Smoother %s did not complete smoothing in specified time limit"
        "(%lf seconds) and was interrupted after %lf seconds",
        current_smoother_.c_str(),
        rclcpp::Duration(goal->max_smoothing_duration).seconds(),
        rclcpp::Duration(result->smoothing_duration).seconds());
    }
    auto msg = std::make_unique<nav_msgs::msg::Path>(result->path);
    plan_publisher_->publish(std::move(msg));
 
    // Check for collisions
    if (goal->check_for_collisions) {
      geometry_msgs::msg::Pose pose;
      bool fetch_data = true;
      for (const auto & p : result->path.poses) {
        pose = p.pose;
 
        if (!collision_checker_->isCollisionFree(pose, fetch_data)) {
          RCLCPP_ERROR(
            get_logger(),
            "Smoothed path leads to a collision at x: %lf, y: %lf, theta: %lf",
            pose.position.x, pose.position.y, tf2::getYaw(pose.orientation));
          throw nav2_core::SmoothedPathInCollision(
                  "Smoothed Path collided at"
                  "X: " + std::to_string(pose.position.x) +
                  "Y: " + std::to_string(pose.position.y) +
                  "Theta: " + std::to_string(tf2::getYaw(pose.orientation)));
        }
        fetch_data = false;
      }
    }
 
    RCLCPP_DEBUG(
      get_logger(), "Smoother succeeded (time: %lf), setting result",
      rclcpp::Duration(result->smoothing_duration).seconds());
 
    action_server_->succeeded_current(result);
  } catch (nav2_core::InvalidSmoother & ex) {
    result->error_msg = ex.what();
    RCLCPP_ERROR(this->get_logger(), "%s", result->error_msg.c_str());
    result->error_code = ActionResult::INVALID_SMOOTHER;
    action_server_->terminate_current(result);
    return;
  } catch (nav2_core::SmootherTimedOut & ex) {
    result->error_msg = ex.what();
    RCLCPP_ERROR(this->get_logger(), "%s", result->error_msg.c_str());
    result->error_code = ActionResult::TIMEOUT;
    action_server_->terminate_current(result);
    return;
  } catch (nav2_core::SmoothedPathInCollision & ex) {
    result->error_msg = ex.what();
    RCLCPP_ERROR(this->get_logger(), "%s", result->error_msg.c_str());
    result->error_code = ActionResult::SMOOTHED_PATH_IN_COLLISION;
    action_server_->terminate_current(result);
    return;
  } catch (nav2_core::FailedToSmoothPath & ex) {
    result->error_msg = ex.what();
    RCLCPP_ERROR(this->get_logger(), "%s", result->error_msg.c_str());
    result->error_code = ActionResult::FAILED_TO_SMOOTH_PATH;
    action_server_->terminate_current(result);
    return;
  } catch (nav2_core::InvalidPath & ex) {
    result->error_msg = ex.what();
    RCLCPP_ERROR(this->get_logger(), "%s", result->error_msg.c_str());
    result->error_code = ActionResult::INVALID_PATH;
    action_server_->terminate_current(result);
    return;
  } catch (nav2_core::SmootherException & ex) {
    result->error_msg = ex.what();
    RCLCPP_ERROR(this->get_logger(), "%s", result->error_msg.c_str());
    result->error_code = ActionResult::UNKNOWN;
    action_server_->terminate_current(result);
    return;
  } catch (std::exception & ex) {
    result->error_msg = ex.what();
    RCLCPP_ERROR(this->get_logger(), "%s", result->error_msg.c_str());
    result->error_code = ActionResult::UNKNOWN;
    action_server_->terminate_current(result);
    return;
  }
}
 
bool SmootherServer::validate(const nav_msgs::msg::Path & path)
{
  if (path.poses.empty()) {
    RCLCPP_WARN(get_logger(), "Requested path to smooth is empty");
    return false;
  }
 
  RCLCPP_DEBUG(get_logger(), "Requested path to smooth is valid");
  return true;
}
 
}  // namespace nav2_smoother
 
#include "rclcpp_components/register_node_macro.hpp"
 
// Register the component with class_loader.
// This acts as a sort of entry point, allowing the component to be discoverable when its library
// is being loaded into a running process.
RCLCPP_COMPONENTS_REGISTER_NODE(nav2_smoother::SmootherServer)