From b78a7c4a95945d4d2153be15247449546f69f248 Mon Sep 17 00:00:00 2001 From: Dave Coleman Date: Thu, 16 Jun 2016 11:19:16 -0600 Subject: [PATCH] Added insertion directly into planning_graph w/o planning --- .../include/descartes_core/path_planner_base.h | 1 - .../include/descartes_planner/dense_planner.h | 4 ++++ descartes_planner/src/dense_planner.cpp | 17 ++++++++++++++++- 3 files changed, 20 insertions(+), 2 deletions(-) diff --git a/descartes_core/include/descartes_core/path_planner_base.h b/descartes_core/include/descartes_core/path_planner_base.h index 127ff621..e6c889aa 100644 --- a/descartes_core/include/descartes_core/path_planner_base.h +++ b/descartes_core/include/descartes_core/path_planner_base.h @@ -50,7 +50,6 @@ class PathPlannerBase /** * @brief Plans a path for the given robot model and configuration parameters. * @param model robot model implementation for which to plan a path - * @param config A map containing the parameter/value pairs. */ virtual bool initialize(RobotModelConstPtr model) = 0; diff --git a/descartes_planner/include/descartes_planner/dense_planner.h b/descartes_planner/include/descartes_planner/dense_planner.h index 9ec76979..1b0651c1 100644 --- a/descartes_planner/include/descartes_planner/dense_planner.h +++ b/descartes_planner/include/descartes_planner/dense_planner.h @@ -26,6 +26,10 @@ class DensePlanner : public descartes_core::PathPlannerBase virtual bool setConfig(const descartes_core::PlannerConfig& config); virtual void getConfig(descartes_core::PlannerConfig& config) const; virtual bool planPath(const std::vector& traj); + + /** \brief Populate a Cartesian trajectory graph, but do not solve. For use by external solvers */ + virtual bool insertGraph(const std::vector& traj); + virtual bool getPath(std::vector& path) const; virtual bool addAfter(const descartes_core::TrajectoryPt::ID& ref_id, descartes_core::TrajectoryPtPtr tp); virtual bool addBefore(const descartes_core::TrajectoryPt::ID& ref_id, descartes_core::TrajectoryPtPtr tp); diff --git a/descartes_planner/src/dense_planner.cpp b/descartes_planner/src/dense_planner.cpp index 7c105183..7b6c10a2 100644 --- a/descartes_planner/src/dense_planner.cpp +++ b/descartes_planner/src/dense_planner.cpp @@ -199,7 +199,6 @@ bool DensePlanner::planPath(const std::vector& return false; } - double cost; path_.clear(); error_code_ = descartes_core::PlannerError::EMPTY_PATH; @@ -215,6 +214,22 @@ bool DensePlanner::planPath(const std::vector& return descartes_core::PlannerError::OK == error_code_; } +bool DensePlanner::insertGraph(const std::vector& traj) +{ + if(error_code_ == descartes_core::PlannerError::UNINITIALIZED) + { + ROS_ERROR_STREAM("Planner has not been initialized"); + return false; + } + + if(!planning_graph_->insertGraph(&traj)) + { + return false; + } + + return true; +} + bool DensePlanner::getPath(std::vector& path) const { if (path_.empty())