dense_planner.cpp

Go to the documentation of this file.

/*
 * dense_planner.cpp
 *
 *  Created on: Feb 9, 2015
 *      Author: ros developer 
 */

#include <descartes_planner/dense_planner.h>

namespace descartes_planner
{

using namespace descartes_core;
DensePlanner::DensePlanner():
    planning_graph_(),
    error_code_(descartes_core::PlannerError::UNINITIALIZED)
{
 error_map_ = {
                {PlannerError::OK,"OK"},
                {PlannerError::EMPTY_PATH,"No path plan has been generated"},
                {PlannerError::INVALID_ID,"ID is nil or isn't part of the path"},
                {PlannerError::IK_NOT_AVAILABLE,"One or more ik solutions could not be found"},
                {PlannerError::UNINITIALIZED,"Planner has not been initialized with a robot model"},
                {PlannerError::INCOMPLETE_PATH,"Input trajectory and output path point cound differ"}
              };
}

DensePlanner::~DensePlanner()
{

}

bool DensePlanner::initialize(descartes_core::RobotModelConstPtr model)
{
  planning_graph_ = boost::shared_ptr<descartes_planner::PlanningGraph>(
      new descartes_planner::PlanningGraph(std::move(model)));
  error_code_ = descartes_core::PlannerErrors::EMPTY_PATH;
  return true;
}

bool DensePlanner::setConfig(const descartes_core::PlannerConfig& config)
{
  config_ = config;
  config_.clear();
  return true;
}

void DensePlanner::getConfig(descartes_core::PlannerConfig& config) const
{
  config = config_;
}

descartes_core::TrajectoryPt::ID DensePlanner::getPrevious(const descartes_core::TrajectoryPt::ID& ref_id)
{
  descartes_core::TrajectoryPt::ID id;
  auto predicate = [&ref_id](descartes_core::TrajectoryPtPtr p)
    {
      return ref_id == p->getID();
    };

  auto pos = std::find_if(path_.begin()++,path_.end(),predicate);
  if(pos == path_.end() )
  {
    id =  descartes_core::TrajectoryID::make_nil();
  }
  else
  {
    pos--;
    id =  (*pos)->getID();
  }

  return id;
}

bool DensePlanner::updatePath()
{
  std::vector<descartes_core::TrajectoryPtPtr> traj;
  const CartesianMap& cart_map = planning_graph_->getCartesianMap();
  descartes_core::TrajectoryPt::ID first_id = descartes_core::TrajectoryID::make_nil();
  auto predicate = [&first_id](const std::pair<descartes_core::TrajectoryPt::ID,CartesianPointInformation>& p)
    {
      const auto& info = p.second;
      if(info.links_.id_previous == descartes_core::TrajectoryID::make_nil())
      {
        first_id = p.first;
        return true;
      }
      else
      {
        return false;
      }
    };

  // finding first point
  if(cart_map.empty()
      || (std::find_if(cart_map.begin(),cart_map.end(),predicate) == cart_map.end())
      || first_id == descartes_core::TrajectoryID::make_nil())
  {
    error_code_ = descartes_core::PlannerError::INVALID_ID;
    return false;
  }

  // retrieving original trajectory
  traj.resize(cart_map.size());
  descartes_core::TrajectoryPt::ID current_id = first_id;
  for(int i = 0; i < traj.size(); i++)
  {
    if(cart_map.count(current_id) == 0)
    {
      ROS_ERROR_STREAM("Trajectory point "<<current_id<<" was not found in cartesian trajectory.");
      return false;
    }
    const CartesianPointInformation& info  = cart_map.at(current_id);
    traj[i] = info.source_trajectory_;
    current_id = info.links_.id_next;
  }

  // updating planned path
  std::list<descartes_trajectory::JointTrajectoryPt> list;
  double cost;
  if(planning_graph_->getShortestPath(cost,list) )
  {
    ROS_INFO_STREAM("Traj size: " << traj.size() << " List size: " << list.size());
    if(traj.size() == list.size())
    {
      error_code_ = descartes_core::PlannerError::OK;

      // reassigning ids
      int counter = 0;
      path_.resize(list.size());
      for(auto p : list)
      {
        path_[counter] = descartes_core::TrajectoryPtPtr(new descartes_trajectory::JointTrajectoryPt(p));
        path_[counter]->setID(traj[counter]->getID());
        counter++;
      }
    }
    else
    {
      error_code_ = descartes_core::PlannerError::INCOMPLETE_PATH;
    }
  }
  else
  {
    error_code_ = descartes_core::PlannerError::IK_NOT_AVAILABLE;
  }

  return error_code_ == descartes_core::PlannerError::OK;
}

descartes_core::TrajectoryPt::ID DensePlanner::getNext(const descartes_core::TrajectoryPt::ID& ref_id)
{
  descartes_core::TrajectoryPt::ID id;
  auto predicate = [&ref_id](descartes_core::TrajectoryPtPtr p)
    {
      return ref_id == p->getID();
    };

  auto pos = std::find_if(path_.begin(),path_.end()-2,predicate);
  if(pos == path_.end() )
  {
    id =  descartes_core::TrajectoryID::make_nil();
  }
  else
  {
    pos++;
    id =  (*pos)->getID();
  }
  return id;
}

descartes_core::TrajectoryPtPtr DensePlanner::get(const descartes_core::TrajectoryPt::ID& ref_id)
{
  descartes_core::TrajectoryPtPtr p;
  auto predicate = [&ref_id](descartes_core::TrajectoryPtPtr p)
    {
      return ref_id == p->getID();
    };

  auto pos = std::find_if(path_.begin(),path_.end()-2,predicate);
  if(pos == path_.end() )
  {
    p.reset();
  }
  else
  {
    p  =  *pos;
  }
  return p;
}

bool DensePlanner::planPath(const std::vector<descartes_core::TrajectoryPtPtr>& traj)
{
  if(error_code_ == descartes_core::PlannerError::UNINITIALIZED)
  {
    ROS_ERROR_STREAM("Planner has not been initialized");
    return false;
  }

  double cost;
  path_.clear();
  error_code_ = descartes_core::PlannerError::EMPTY_PATH;

  if(planning_graph_->insertGraph(&traj))
  {
    updatePath();
  }
  else
  {
    error_code_ = descartes_core::PlannerError::IK_NOT_AVAILABLE;
  }

  return descartes_core::PlannerError::OK == error_code_;
}

bool DensePlanner::getPath(std::vector<descartes_core::TrajectoryPtPtr>& path) const
{
  if (path_.empty()) return false;
  
  path.assign(path_.begin(),path_.end());
  return error_code_ == descartes_core::PlannerError::OK;
}

bool DensePlanner::addAfter(const descartes_core::TrajectoryPt::ID& ref_id, descartes_core::TrajectoryPtPtr tp)
{
  if(path_.empty())
  {
    return false;
  }

  descartes_core::TrajectoryPt::ID next_id = getNext(ref_id);
  if(!next_id.is_nil())
  {
    if(planning_graph_->addTrajectory(tp,ref_id,next_id))
    {
      if(updatePath())
      {
        error_code_ = descartes_core::PlannerError::OK;
      }
      else
      {
        return false;
      }
    }
    else
    {
      error_code_ = descartes_core::PlannerErrors::IK_NOT_AVAILABLE;
      return false;
    }
  }
  else
  {
    error_code_ = descartes_core::PlannerError::INVALID_ID;
    return false;
  }

  return true;
}

bool DensePlanner::addBefore(const descartes_core::TrajectoryPt::ID& ref_id, descartes_core::TrajectoryPtPtr tp)
{
  if(path_.empty())
  {
    return false;
  }

  descartes_core::TrajectoryPt::ID prev_id = getPrevious(ref_id);
  if(!prev_id.is_nil())
  {
    if(planning_graph_->addTrajectory(tp,prev_id,ref_id))
    {
      if(updatePath())
      {
        error_code_ = descartes_core::PlannerError::OK;
      }
      else
      {
        return false;
      }
    }
    else
    {
      error_code_ = descartes_core::PlannerErrors::IK_NOT_AVAILABLE;
      return false;
    }
  }
  else
  {
    error_code_ = descartes_core::PlannerError::INVALID_ID;
    return false;
  }

  return true;
}

bool DensePlanner::remove(const descartes_core::TrajectoryPt::ID& ref_id)
{
  if(path_.empty())
  {
    return false;
  }

  descartes_core::TrajectoryPtPtr tp = get(ref_id);
  if(tp)
  {
    tp->setID(ref_id);
    if(planning_graph_->removeTrajectory(tp))
    {
      if(updatePath())
      {
        error_code_ = descartes_core::PlannerError::OK;
      }
      else
      {
        return false;
      }
    }
    else
    {
      error_code_ = descartes_core::PlannerErrors::IK_NOT_AVAILABLE;
      return false;
    }
  }
  else
  {
    error_code_ = descartes_core::PlannerError::INVALID_ID;
    return false;
  }

  return true;
}

bool DensePlanner::modify(const descartes_core::TrajectoryPt::ID& ref_id, descartes_core::TrajectoryPtPtr tp)
{
  if(path_.empty())
  {
    return false;
  }

  if(!ref_id.is_nil())
  {
    tp->setID(ref_id);
    if(planning_graph_->modifyTrajectory(tp))
    {
      if(updatePath())
      {
        error_code_ = descartes_core::PlannerError::OK;
      }
      else
      {
        return false;
      }
    }
    else
    {
      error_code_ = descartes_core::PlannerErrors::IK_NOT_AVAILABLE;
      return false;
    }
  }
  else
  {
    error_code_ = descartes_core::PlannerError::INVALID_ID;
    return false;
  }

  return true;
}

int DensePlanner::getErrorCode() const
{
  return error_code_;
}

bool DensePlanner::getErrorMessage(int error_code, std::string& msg) const
{
  std::map<int,std::string>::const_iterator it = error_map_.find(error_code);

  if(it != error_map_.cend())
  {
    msg = it->second;
  }
  else
  {
    return false;
  }
  return true;
}


} /* namespace descartes_core */