custom_nav2_planner.cpp

/*
This piece of work has been modified from the nav2 straight_line_planner.cpp.
Original licence below:
*/

/*********************************************************************
 *
 * Software License Agreement (BSD License)
 *
 *  Copyright (c) 2020 Shivang Patel
 *  All rights reserved.
 *
 *  Redistribution and use in source and binary forms, with or without
 *  modification, are permitted provided that the following conditions
 *  are met:
 *
 *   * Redistributions of source code must retain the above copyright
 *     notice, this list of conditions and the following disclaimer.
 *   * Redistributions in binary form must reproduce the above
 *     copyright notice, this list of conditions and the following
 *     disclaimer in the documentation and/or other materials provided
 *     with the distribution.
 *   * Neither the name of Willow Garage, Inc. nor the names of its
 *     contributors may be used to endorse or promote products derived
 *     from this software without specific prior written permission.
 *
 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 *  POSSIBILITY OF SUCH DAMAGE.
 *
 * Author: Shivang Patel
 *
 * Reference tutorial:
 * https://navigation.ros.org/tutorials/docs/writing_new_nav2planner_plugin.html
 *********************************************************************/

#include <cmath>
#include <string>
#include <memory>
#include <chrono>
#include "nav2_util/node_utils.hpp"

#include "custom_nav2_planner/custom_nav2_planner.hpp"
#include "create_plan_msgs/srv/create_plan.hpp"

using namespace std::chrono_literals;

namespace custom_nav2_planner
{

void CustomPlanner::configure(
  const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent,
  std::string name, std::shared_ptr<tf2_ros::Buffer> tf,
  std::shared_ptr<nav2_costmap_2d::Costmap2DROS> costmap_ros)
{
  node_ = parent.lock();
  name_ = name;
  tf_ = tf;
  costmap_ = costmap_ros->getCostmap();
  global_frame_ = costmap_ros->getGlobalFrameID();
}

void CustomPlanner::cleanup()
{
  RCLCPP_INFO(
    node_->get_logger(), "CleaningUp plugin %s of type CustomPlanner",
    name_.c_str());
}

void CustomPlanner::activate()
{
  RCLCPP_INFO(
    node_->get_logger(), "Activating plugin %s of type CustomPlanner",
    name_.c_str());
}

void CustomPlanner::deactivate()
{
  RCLCPP_INFO(
    node_->get_logger(), "Deactivating plugin %s of type CustomPlanner",
    name_.c_str());
}

nav_msgs::msg::Path CustomPlanner::createPlan(
  const geometry_msgs::msg::PoseStamped & start,
  const geometry_msgs::msg::PoseStamped & goal)
{
  nav_msgs::msg::Path global_path;

  // Checking if the goal and start state is in the global frame
  if (start.header.frame_id != global_frame_) {
    RCLCPP_ERROR(
      node_->get_logger(), "Planner will only accept start position from %s frame",
      global_frame_.c_str());
    return global_path;
  }

  if (goal.header.frame_id != global_frame_) {
    RCLCPP_INFO(
      node_->get_logger(), "Planner will only accept goal position from %s frame",
      global_frame_.c_str());
    return global_path;
  }

  // Create new client to request a plan from "create_plan" service
  rclcpp::Client<create_plan_msgs::srv::CreatePlan>::SharedPtr client =
    node_->create_client<create_plan_msgs::srv::CreatePlan>("create_plan");

  auto request = std::make_shared<create_plan_msgs::srv::CreatePlan::Request>();
  request->start = start;
  request->goal = goal;

  global_path.poses.clear();
  global_path.header.stamp = node_->now();
  global_path.header.frame_id = global_frame_;

  if (!client->wait_for_service(1s)) {
    RCLCPP_WARN(rclcpp::get_logger("rclcpp"), "create_plan service not available.");
    return global_path;
  }

  auto future = client->async_send_request(request);

  // Wait for the result.
  std::future_status status;
  auto timeout_duration = std::chrono::seconds(1);
  auto start_time = std::chrono::steady_clock::now();

  do {
      status = future.wait_for(std::chrono::milliseconds(100));
      if (status == std::future_status::ready) {
          global_path.poses = future.get()->path.poses;
          RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "Got path as a service result");
          break;
      }
      // Check if the timeout has been reached
      if (std::chrono::steady_clock::now() - start_time > timeout_duration) {
          RCLCPP_ERROR(rclcpp::get_logger("rclcpp"), "Timeout reached while waiting for service result");
          return global_path;
      }
  } while (rclcpp::ok());
  
  return global_path;
}

}  // namespace custom_nav2_planner

#include "pluginlib/class_list_macros.hpp"
PLUGINLIB_EXPORT_CLASS(custom_nav2_planner::CustomPlanner, nav2_core::GlobalPlanner)