src/abstract_planner_execution.cpp

Go to the documentation of this file.

/*
 *  Copyright 2018, Magazino GmbH, Sebastian Pütz, Jorge Santos Simón
 *
 *  Redistribution and use in source and binary forms, with or without
 *  modification, are permitted provided that the following conditions
 *  are met:
 *
 *  1. Redistributions of source code must retain the above copyright
 *     notice, this list of conditions and the following disclaimer.
 *
 *  2. 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.
 *
 *  3. Neither the name of the copyright holder 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 HOLDER 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.
 *
 *  abstract_planner_execution.cpp
 *
 *  authors:
 *    Sebastian Pütz <spuetz@uni-osnabrueck.de>
 *    Jorge Santos Simón <santos@magazino.eu>
 *
 */

#include "mbf_abstract_nav/abstract_planner_execution.h"

namespace mbf_abstract_nav
{
AbstractPlannerExecution::AbstractPlannerExecution(const std::string& name,
                                                   const mbf_abstract_core::AbstractPlanner::Ptr& planner_ptr,
                                                   const MoveBaseFlexConfig& config)
  : AbstractExecutionBase(name)
  , planner_(planner_ptr)
  , state_(INITIALIZED)
  , max_retries_(0)
  , planning_(false)
  , has_new_start_(false)
  , has_new_goal_(false)
{
  ros::NodeHandle private_nh("~");

  // non-dynamically reconfigurable parameters
  private_nh.param("robot_frame", robot_frame_, std::string("base_footprint"));
  private_nh.param("map_frame", global_frame_, std::string("map"));

  // dynamically reconfigurable parameters
  reconfigure(config);
}

AbstractPlannerExecution::AbstractPlannerExecution(const std::string& name,
                                                   const mbf_abstract_core::AbstractPlanner::Ptr& planner_ptr,
                                                   const TFPtr& tf_listener_ptr, const MoveBaseFlexConfig& config)
  : AbstractExecutionBase(name)
  , planner_(planner_ptr)
  , state_(INITIALIZED)
  , max_retries_(0)
  , planning_(false)
  , tf_listener_ptr_(tf_listener_ptr)
  , has_new_start_(false)
  , has_new_goal_(false)
{
  ros::NodeHandle private_nh("~");

  // non-dynamically reconfigurable parameters
  private_nh.param("robot_frame", robot_frame_, std::string("base_footprint"));
  private_nh.param("map_frame", global_frame_, std::string("map"));

  // dynamically reconfigurable parameters
  reconfigure(config);
}

AbstractPlannerExecution::~AbstractPlannerExecution()
{
}

template <typename _Iter>
double sumDistance(_Iter _begin, _Iter _end)
{
  // helper function to get the distance of a path.
  // in C++11, we could add static_assert on the interator_type.
  double dist = 0.;

  // minimum length of the path is 2.
  if (std::distance(_begin, _end) < 2)
    return dist;

  // two pointer iteration
  for (_Iter next = _begin + 1; next != _end; ++_begin, ++next)
    dist += mbf_utility::distance(*_begin, *next);

  return dist;
}

double AbstractPlannerExecution::getCost() const
{
  return cost_;
}

void AbstractPlannerExecution::reconfigure(const MoveBaseFlexConfig &config)
{
  boost::lock_guard<boost::mutex> guard(configuration_mutex_);

  max_retries_ = config.planner_max_retries;
  frequency_ = config.planner_frequency;

  // Timeout granted to the global planner. We keep calling it up to this time or up to max_retries times
  // If it doesn't return within time, the navigator will cancel it and abort the corresponding action
  try
  {
    patience_ = ros::Duration(config.planner_patience);
  }
  catch (std::exception& ex)
  {
    ROS_ERROR_STREAM("Failed to set planner_patience: " << ex.what());
    patience_ = ros::Duration(0);
  }
}


typename AbstractPlannerExecution::PlanningState AbstractPlannerExecution::getState() const
{
  boost::lock_guard<boost::mutex> guard(state_mtx_);
  return state_;
}

void AbstractPlannerExecution::setState(PlanningState state, bool signalling)
{
  boost::lock_guard<boost::mutex> guard(state_mtx_);
  state_ = state;

  // we exit planning if we are signalling.
  planning_ = !signalling;

  // some states are quiet, most aren't
  if(signalling)
    condition_.notify_all();
}


ros::Time AbstractPlannerExecution::getLastValidPlanTime() const
{
  boost::lock_guard<boost::mutex> guard(plan_mtx_);
  return last_valid_plan_time_;
}


bool AbstractPlannerExecution::isPatienceExceeded() const
{
  return !patience_.isZero() && (ros::Time::now() - last_call_start_time_ > patience_);
}


std::vector<geometry_msgs::PoseStamped> AbstractPlannerExecution::getPlan() const
{
  boost::lock_guard<boost::mutex> guard(plan_mtx_);
  // copy plan and costs to output
  return plan_;
}


void AbstractPlannerExecution::setNewGoal(const geometry_msgs::PoseStamped &goal, double tolerance)
{
  boost::lock_guard<boost::mutex> guard(goal_start_mtx_);
  goal_ = goal;
  tolerance_ = tolerance;
  has_new_goal_ = true;
}


void AbstractPlannerExecution::setNewStart(const geometry_msgs::PoseStamped &start)
{
  boost::lock_guard<boost::mutex> guard(goal_start_mtx_);
  start_ = start;
  has_new_start_ = true;
}


void AbstractPlannerExecution::setNewStartAndGoal(const geometry_msgs::PoseStamped &start,
                                                  const geometry_msgs::PoseStamped &goal,
                                                  double tolerance)
{
  boost::lock_guard<boost::mutex> guard(goal_start_mtx_);
  start_ = start;
  goal_ = goal;
  tolerance_ = tolerance;
  has_new_start_ = true;
  has_new_goal_ = true;
}


bool AbstractPlannerExecution::start(const geometry_msgs::PoseStamped &start,
                                     const geometry_msgs::PoseStamped &goal,
                                     double tolerance)
{
  if (planning_)
  {
    return false;
  }
  boost::lock_guard<boost::mutex> guard(planning_mtx_);
  planning_ = true;
  start_ = start;
  goal_ = goal;
  tolerance_ = tolerance;

  const geometry_msgs::Point& s = start.pose.position;
  const geometry_msgs::Point& g = goal.pose.position;

  ROS_DEBUG_STREAM("Start planning from the start pose: (" << s.x << ", " << s.y << ", " << s.z << ")"
                                 << " to the goal pose: ("<< g.x << ", " << g.y << ", " << g.z << ")");

  return AbstractExecutionBase::start();
}


bool AbstractPlannerExecution::cancel()
{
  cancel_ = true; // force cancel immediately, as the call to cancel in the planner can take a while

  // returns false if cancel is not implemented or rejected by the planner (will run until completion)
  if (!planner_->cancel())
  {
    ROS_WARN_STREAM("Cancel planning failed or is not supported by the plugin. "
        << "Wait until the current planning finished!");

    return false;
  }
  return true;
}

uint32_t AbstractPlannerExecution::makePlan(const geometry_msgs::PoseStamped &start,
                                            const geometry_msgs::PoseStamped &goal,
                                            double tolerance,
                                            std::vector<geometry_msgs::PoseStamped> &plan,
                                            double &cost,
                                            std::string &message)
{
  return planner_->makePlan(start, goal, tolerance, plan, cost, message);
}

void AbstractPlannerExecution::run()
{
  setState(STARTED, false);
  boost::lock_guard<boost::mutex> guard(planning_mtx_);
  int retries = 0;
  geometry_msgs::PoseStamped current_start = start_;
  geometry_msgs::PoseStamped current_goal = goal_;
  double current_tolerance = tolerance_;

  last_call_start_time_ = ros::Time::now();
  last_valid_plan_time_ = ros::Time::now();

  try
  {
    while (planning_ && ros::ok())
    {
      // call the planner
      std::vector<geometry_msgs::PoseStamped> plan;
      double cost;

      // lock goal start mutex
      goal_start_mtx_.lock();
      if (has_new_start_)
      {
        has_new_start_ = false;
        current_start = start_;
        ROS_INFO_STREAM("A new start pose is available. Planning with the new start pose!");
        const geometry_msgs::Point& s = start_.pose.position;
        ROS_INFO_STREAM("New planning start pose: (" << s.x << ", " << s.y << ", " << s.z << ")");
      }
      if (has_new_goal_)
      {
        has_new_goal_ = false;
        current_goal = goal_;
        current_tolerance = tolerance_;
        ROS_INFO_STREAM("A new goal pose is available. Planning with the new goal pose and the tolerance: "
                        << current_tolerance);
        const geometry_msgs::Point& g = goal_.pose.position;
        ROS_INFO_STREAM("New goal pose: (" << g.x << ", " << g.y << ", " << g.z << ")");
      }

      // unlock goal
      goal_start_mtx_.unlock();
      if (cancel_)
      {
        ROS_INFO_STREAM("The global planner has been canceled!");
        setState(CANCELED, true);
      }
      else
      {
        setState(PLANNING, false);

        outcome_ = makePlan(current_start, current_goal, current_tolerance, plan, cost, message_);
        bool success = outcome_ < 10;

        boost::lock_guard<boost::mutex> guard(configuration_mutex_);

        if (cancel_ && !isPatienceExceeded())
        {
          ROS_INFO_STREAM("The planner \"" << name_ << "\" has been canceled!"); // but not due to patience exceeded
          setState(CANCELED, true);
        }
        else if (success)
        {
          ROS_DEBUG_STREAM("Successfully found a plan.");

          boost::lock_guard<boost::mutex> plan_mtx_guard(plan_mtx_);
          plan_ = plan;
          cost_ = cost;
          // estimate the cost based on the distance if its zero.
          if (cost_ == 0)
            cost_ = sumDistance(plan_.begin(), plan_.end());

          last_valid_plan_time_ = ros::Time::now();
          setState(FOUND_PLAN, true);
        }
        else if (max_retries_ > 0 && ++retries > max_retries_)
        {
          ROS_INFO_STREAM("Planning reached max retries! (" << max_retries_ << ")");
          setState(MAX_RETRIES, true);
        }
        else if (isPatienceExceeded())
        {
          // Patience exceeded is handled at two levels: here to stop retrying planning when max_retries is
          // disabled, and on the navigation server when the planner doesn't return for more that patience seconds.
          // In the second case, the navigation server has tried to cancel planning (possibly without success, as
          // old nav_core-based planners do not support canceling), and we add here the fact to the log for info
          ROS_INFO_STREAM("Planning patience (" << patience_.toSec() << "s) has been exceeded"
                                                << (cancel_ ? "; planner canceled!" : ""));
          setState(PAT_EXCEEDED, true);
        }
        else if (max_retries_ == 0 && patience_.isZero())
        {
          ROS_INFO_STREAM("Planning could not find a plan!");
          setState(NO_PLAN_FOUND, true);
        }
        else
        {
          ROS_DEBUG_STREAM("Planning could not find a plan! Trying again...");
        }
      }
    } // while (planning_ && ros::ok())
  }
  catch (const boost::thread_interrupted &ex)
  {
    // Planner thread interrupted; probably we have exceeded planner patience
    ROS_WARN_STREAM("Planner thread interrupted!");
    setState(STOPPED, true);
  }
  catch (...)
  {
    ROS_ERROR_STREAM("Unknown error occurred: " << boost::current_exception_diagnostic_information());
    setState(INTERNAL_ERROR, true);
  }
}

} /* namespace mbf_abstract_nav */