sparse_planner.h
Go to the documentation of this file.
00001 /*
00002  * Software License Agreement (Apache License)
00003  *
00004  * Copyright (c) 2014, Southwest Research Institute
00005  *
00006  * Licensed under the Apache License, Version 2.0 (the "License");
00007  * you may not use this file except in compliance with the License.
00008  * You may obtain a copy of the License at
00009  *
00010  * http://www.apache.org/licenses/LICENSE-2.0
00011  *
00012  * Unless required by applicable law or agreed to in writing, software
00013  * distributed under the License is distributed on an "AS IS" BASIS,
00014  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
00015  * See the License for the specific language governing permissions and
00016  * limitations under the License.
00017  */
00018 /*
00019  * sparse_planner.h
00020  *
00021  *  Created on: Dec 17, 2014
00022  *      Author: ros developer
00023  */
00024 
00025 #ifndef SPARSE_PLANNER_H_
00026 #define SPARSE_PLANNER_H_
00027 
00028 #include <descartes_core/path_planner_base.h>
00029 #include <descartes_planner/planning_graph.h>
00030 #include <tuple>
00031 
00032 namespace descartes_planner
00033 {
00034 class SparsePlanner : public descartes_core::PathPlannerBase
00035 {
00036 public:
00037   typedef std::vector<std::tuple<int, descartes_core::TrajectoryPtPtr, descartes_trajectory::JointTrajectoryPt> >
00038       SolutionArray;
00039 
00040 public:
00041   SparsePlanner(descartes_core::RobotModelConstPtr model, double sampling = 0.1f);
00042   SparsePlanner();
00043   virtual ~SparsePlanner();
00044 
00045   virtual bool initialize(descartes_core::RobotModelConstPtr model);
00046   virtual bool initialize(descartes_core::RobotModelConstPtr model,
00047                           descartes_planner::CostFunction cost_function_callback);
00048   virtual bool setConfig(const descartes_core::PlannerConfig& config);
00049   virtual void getConfig(descartes_core::PlannerConfig& config) const;
00050   virtual bool planPath(const std::vector<descartes_core::TrajectoryPtPtr>& traj);
00051   virtual bool addAfter(const descartes_core::TrajectoryPt::ID& ref_id, descartes_core::TrajectoryPtPtr cp);
00052   virtual bool addBefore(const descartes_core::TrajectoryPt::ID& ref_id, descartes_core::TrajectoryPtPtr cp);
00053   virtual bool modify(const descartes_core::TrajectoryPt::ID& ref_id, descartes_core::TrajectoryPtPtr cp);
00054   virtual bool remove(const descartes_core::TrajectoryPt::ID& ref_id);
00055   virtual bool getPath(std::vector<descartes_core::TrajectoryPtPtr>& path) const;
00056   virtual int getErrorCode() const;
00057   virtual bool getErrorMessage(int error_code, std::string& msg) const;
00058 
00059   void setSampling(double sampling);
00060   const std::map<descartes_core::TrajectoryPt::ID, descartes_trajectory::JointTrajectoryPt>& getSolution();
00061   bool getSolutionJointPoint(const descartes_trajectory::CartTrajectoryPt::ID& cart_id,
00062                              descartes_trajectory::JointTrajectoryPt& j);
00063 
00064 protected:
00065   bool plan();
00066   bool interpolateJointPose(const std::vector<double>& start, const std::vector<double>& end, double t,
00067                             std::vector<double>& interp);
00068   int interpolateSparseTrajectory(const SolutionArray& sparse_solution, int& sparse_index, int& point_pos);
00069   void sampleTrajectory(double sampling, const std::vector<descartes_core::TrajectoryPtPtr>& dense_trajectory_array,
00070                         std::vector<descartes_core::TrajectoryPtPtr>& sparse_trajectory_array);
00071 
00072   int getDensePointIndex(const descartes_core::TrajectoryPt::ID& ref_id);
00073   int getSparsePointIndex(const descartes_core::TrajectoryPt::ID& ref_id);
00074   int findNearestSparsePointIndex(const descartes_core::TrajectoryPt::ID& ref_id, bool skip_equal = true);
00075   bool isInSparseTrajectory(const descartes_core::TrajectoryPt::ID& ref_id);
00076   bool checkJointChanges(const std::vector<double>& s1, const std::vector<double>& s2, const double& max_change);
00077 
00078   bool getOrderedSparseArray(std::vector<descartes_core::TrajectoryPtPtr>& sparse_array);
00079   bool getSparseSolutionArray(SolutionArray& sparse_solution_array);
00080 
00081 protected:
00082   enum class InterpolationResult : int
00083   {
00084     ERROR = -1,
00085     REPLAN,
00086     SUCCESS
00087   };
00088 
00089   double sampling_;
00090   int error_code_;
00091   std::map<int, std::string> error_map_;
00092   descartes_core::PlannerConfig config_;
00093   boost::shared_ptr<PlanningGraph> planning_graph_;
00094   std::vector<descartes_core::TrajectoryPtPtr> cart_points_;
00095   SolutionArray sparse_solution_array_;
00096   std::map<descartes_core::TrajectoryPt::ID, descartes_trajectory::JointTrajectoryPt> joint_points_map_;
00097   std::vector<descartes_core::TimingConstraint> timing_cache_;
00098 };
00099 
00100 } /* namespace descartes_planner */
00101 #endif /* SPARSE_PLANNER_H_ */


descartes_planner
Author(s): Jorge Nicho
autogenerated on Thu Jun 6 2019 21:36:12