footstep_graph.h
Go to the documentation of this file.
00001 // -*- mode: c++ -*-
00002 /*********************************************************************
00003  * Software License Agreement (BSD License)
00004  *
00005  *  Copyright (c) 2015, JSK Lab
00006  *  All rights reserved.
00007  *
00008  *  Redistribution and use in source and binary forms, with or without
00009  *  modification, are permitted provided that the following conditions
00010  *  are met:
00011  *
00012  *   * Redistributions of source code must retain the above copyright
00013  *     notice, this list of conditions and the following disclaimer.
00014  *   * Redistributions in binary form must reproduce the above
00015  *     copyright notice, this list of conditions and the following
00016  *     disclaimer in the documentation and/o2r other materials provided
00017  *     with the distribution.
00018  *   * Neither the name of the JSK Lab nor the names of its
00019  *     contributors may be used to endorse or promote products derived
00020  *     from this software without specific prior written permission.
00021  *
00022  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00023  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00024  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00025  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00026  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00027  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00028  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00029  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00030  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00031  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00032  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00033  *  POSSIBILITY OF SUCH DAMAGE.
00034  *********************************************************************/
00035 
00036 
00037 #ifndef JSK_FOOTSTEP_PLANNER_FOOTSTEP_GRAPH_H_
00038 #define JSK_FOOTSTEP_PLANNER_FOOTSTEP_GRAPH_H_
00039 
00040 #include <ros/ros.h>
00041 #include <pcl_ros/point_cloud.h>
00042 #include <jsk_footstep_msgs/FootstepArray.h>
00043 
00044 #include "jsk_recognition_utils/geo_util.h"
00045 #include "jsk_footstep_planner/graph.h"
00046 #include "jsk_footstep_planner/footstep_state.h"
00047 #include "jsk_footstep_planner/astar_solver.h"
00048 #include "jsk_footstep_planner/ann_grid.h"
00049 #include "jsk_footstep_planner/transition_limit.h"
00050 #include "jsk_footstep_planner/footstep_parameters.h"
00051 
00052 namespace jsk_footstep_planner
00053 {
00054   class FootstepGraph: public Graph<FootstepState>
00055   {
00056   public:
00057     typedef boost::shared_ptr<FootstepGraph> Ptr;
00058     typedef typename boost::function< bool(StatePtr target_state, std::vector<StatePtr> &) > SuccessorFunction;
00059     typedef typename boost::function< double(StatePtr, StatePtr, double) > PathCostFunction;
00060 
00061     friend double footstepHeuristicFollowPathLine(SolverNode<FootstepState, FootstepGraph>::Ptr node,
00062                                                   FootstepGraph::Ptr graph);
00063     FootstepGraph(const Eigen::Vector3f& resolution,
00064                   const bool use_pointcloud_model = false,
00065                   const bool lazy_projection = true,
00066                   const bool local_movement = false,
00067                   const bool use_obstacle_model = false):
00068       max_successor_distance_(0.0), max_successor_rotation_(0.0),
00069       publish_progress_(false),
00070       resolution_(resolution),
00071       use_pointcloud_model_(use_pointcloud_model),
00072       lazy_projection_(lazy_projection),
00073       local_movement_(local_movement),
00074       use_obstacle_model_(use_obstacle_model),
00075       pointcloud_model_2d_(new pcl::PointCloud<pcl::PointNormal>),
00076       tree_model_(new pcl::KdTreeFLANN<pcl::PointNormal>),
00077       obstacle_tree_model_(new pcl::KdTreeFLANN<pcl::PointXYZ>),
00078       tree_model_2d_(new pcl::search::Octree<pcl::PointNormal>(0.2)),
00079       grid_search_(new ANNGrid(0.05)),
00080       parameters_(),
00081       zero_state_(new FootstepState(0,
00082                                     Eigen::Affine3f::Identity(),
00083                                     Eigen::Vector3f::UnitX(),
00084                                     resolution_)),
00085       perception_duration_(0.0)
00086     {
00087     }
00088     virtual std::vector<StatePtr> successors(StatePtr target_state) {
00089       std::vector<StatePtr> ret;
00090       successor_func_(target_state, ret);
00091       return ret;
00092     }
00093 
00094     virtual bool isGoal(StatePtr state);
00095 
00096     virtual double pathCost(StatePtr from, StatePtr to, double prev_cost)
00097     {
00098       return path_cost_func_(from, to, prev_cost);
00099     }
00100 
00105     virtual bool isColliding(StatePtr current_state, StatePtr previous_state);
00106     virtual pcl::PointIndices::Ptr getPointIndicesCollidingSphere(const Eigen::Affine3f& c);
00107     virtual bool isCollidingBox(const Eigen::Affine3f& c, pcl::PointIndices::Ptr candidates) const;
00113     virtual Eigen::Affine3f getRobotCoords(StatePtr current_state, StatePtr previous_state) const;
00114     virtual void setBasicSuccessors(
00115       std::vector<Eigen::Affine3f> left_to_right_successors);
00116     
00117     virtual void setGoalState(
00118       FootstepState::Ptr left, FootstepState::Ptr right)
00119     {
00120       left_goal_state_ = left;
00121       right_goal_state_ = right;
00122     }
00123 
00124     virtual void setLeftGoalState(FootstepState::Ptr goal)
00125     {
00126       left_goal_state_ = goal;
00127     }
00128     
00129     virtual void setRightGoalState(FootstepState::Ptr goal)
00130     {
00131       right_goal_state_ = goal;
00132     }
00133 
00138     virtual std::string infoString() const;
00139     
00140     bool finalizeSteps(const StatePtr &last_1_Step, const StatePtr &lastStep,
00141                        std::vector<StatePtr> &finalizeSteps);
00142 
00143     virtual FootstepState::Ptr getGoal(int leg)
00144     {
00145       if (leg == jsk_footstep_msgs::Footstep::LEFT) {
00146         return left_goal_state_;
00147       }
00148       else if (leg == jsk_footstep_msgs::Footstep::RIGHT) {
00149         return right_goal_state_;
00150       }
00151       else {                    // TODO: error
00152         return goal_state_;
00153       }
00154     }
00155 
00156     virtual double maxSuccessorDistance()
00157     {
00158       return max_successor_distance_;
00159     }
00160     virtual double maxSuccessorRotation()
00161     {
00162       return max_successor_rotation_;
00163     }
00164 
00165     virtual void setProgressPublisher(ros::NodeHandle& nh, std::string topic)
00166     {
00167       publish_progress_ = true;
00168       pub_progress_ = nh.advertise<jsk_footstep_msgs::FootstepArray>(topic, 1);
00169     }
00170 
00171     virtual void setPointCloudModel(pcl::PointCloud<pcl::PointNormal>::Ptr model)
00172     {
00173       pointcloud_model_ = model;
00174       tree_model_->setInputCloud(pointcloud_model_);
00175       // Project point_cloud_model_
00176       pcl::ProjectInliers<pcl::PointNormal> proj;
00177       proj.setModelType(pcl::SACMODEL_PLANE);
00178       pcl::ModelCoefficients::Ptr
00179         plane_coefficients (new pcl::ModelCoefficients);
00180       plane_coefficients->values.resize(4.0);
00181       plane_coefficients->values[2] = 1.0; // TODO: configurable
00182       proj.setModelCoefficients(plane_coefficients);
00183       proj.setInputCloud(pointcloud_model_);
00184       proj.filter(*pointcloud_model_2d_);
00185       tree_model_2d_->setInputCloud(pointcloud_model_2d_);
00186       grid_search_->build(*model);
00187     }
00188 
00189     virtual void setObstacleModel(pcl::PointCloud<pcl::PointXYZ>::Ptr model)
00190     {
00191       obstacle_model_ = model;
00192       obstacle_tree_model_->setInputCloud(obstacle_model_);
00193     }
00194     
00195     virtual bool projectGoal();
00196     virtual bool projectStart();
00197     virtual bool isSuccessable(StatePtr current_state, StatePtr previous_state);
00198     virtual bool useObstacleModel() const { return use_obstacle_model_; }
00199     virtual bool usePointCloudModel() const { return use_pointcloud_model_; }
00200     virtual bool lazyProjection()  const { return lazy_projection_; }
00201     virtual bool localMovement() const { return local_movement_; }
00202 
00203     virtual void setParameters (FootstepParameters &p) { parameters_ = p; }
00204     virtual void setTransitionLimit(TransitionLimit::Ptr limit) { transition_limit_ = limit; }
00205     virtual TransitionLimit::Ptr getTransitionLimit() { return transition_limit_; }
00206     virtual void setGlobalTransitionLimit(TransitionLimit::Ptr limit) { global_transition_limit_ = limit; }
00207     virtual TransitionLimit::Ptr getGlobalTransitionLimit() { return global_transition_limit_; }
00208     virtual FootstepState::Ptr projectFootstep(FootstepState::Ptr in);
00209     virtual FootstepState::Ptr projectFootstep(FootstepState::Ptr in, unsigned int& state);
00210     virtual ros::WallDuration getPerceptionDuration() { return perception_duration_; }
00211     virtual void clearPerceptionDuration() { perception_duration_ = ros::WallDuration(0.0); }
00212     virtual std::vector<FootstepState::Ptr> localMoveFootstepState(FootstepState::Ptr in);
00213     virtual void setCollisionBBoxOffset(const Eigen::Affine3f& offset) { collision_bbox_offset_ = offset; }
00214     virtual void setCollisionBBoxSize(const Eigen::Vector3f& size) { collision_bbox_size_ = size; }
00215 
00216     virtual void setSuccessorFunction(SuccessorFunction s) {
00217       successor_func_ = s;
00218     }
00219     virtual void setPathCostFunction(PathCostFunction p) {
00220       path_cost_func_ = p;
00221     }
00222     bool successors_original(StatePtr target_state, std::vector<FootstepGraph::StatePtr> &ret);
00223     double path_cost_original(StatePtr from, StatePtr to, double prev_cost) {
00224       // path cost is a number of steps
00225       return prev_cost + 1;
00226     }
00227     void setHeuristicPathLine(jsk_recognition_utils::PolyLine &path_line)
00228     {
00229       heuristic_path_.reset(new jsk_recognition_utils::PolyLine(path_line)); // copy ???
00230     }
00231   protected:
00232     pcl::PointCloud<pcl::PointNormal>::Ptr pointcloud_model_;
00233     pcl::PointCloud<pcl::PointXYZ>::Ptr obstacle_model_;
00234     pcl::PointCloud<pcl::PointNormal>::Ptr pointcloud_model_2d_;
00235     pcl::KdTreeFLANN<pcl::PointNormal>::Ptr tree_model_;
00236     pcl::KdTreeFLANN<pcl::PointXYZ>::Ptr obstacle_tree_model_;
00237     //pcl::KdTreeFLANN<pcl::PointNormal>::Ptr tree_model_2d_;
00238     pcl::search::Octree<pcl::PointNormal>::Ptr tree_model_2d_;
00239     ANNGrid::Ptr grid_search_;
00240     std::vector<Eigen::Affine3f> successors_from_left_to_right_;
00241     std::vector<Eigen::Affine3f> successors_from_right_to_left_;
00242     FootstepState::Ptr left_goal_state_;
00243     FootstepState::Ptr right_goal_state_;
00248     FootstepState::Ptr zero_state_;
00249     Eigen::Affine3f collision_bbox_offset_;
00250     Eigen::Vector3f collision_bbox_size_;
00251     double max_successor_distance_;
00252     double max_successor_rotation_;
00253     bool publish_progress_;
00254     ros::Publisher pub_progress_;
00255     // const params
00256     const bool use_pointcloud_model_;
00257     const bool lazy_projection_;
00258     const bool local_movement_;
00259     const bool use_obstacle_model_;
00260     const Eigen::Vector3f resolution_;
00261     // params
00262     TransitionLimit::Ptr transition_limit_;
00263     TransitionLimit::Ptr global_transition_limit_;
00264     FootstepParameters parameters_;
00265 
00266     ros::WallDuration perception_duration_;
00267 
00268     // add hint for huristic
00269     jsk_recognition_utils::PolyLine::Ptr heuristic_path_;
00270   private:
00271     SuccessorFunction successor_func_;
00272     PathCostFunction  path_cost_func_;
00273   };
00274 
00275   // heuristic function
00276   double footstepHeuristicZero(
00277     SolverNode<FootstepState, FootstepGraph>::Ptr node, FootstepGraph::Ptr graph);
00278   double footstepHeuristicStraight(
00279     SolverNode<FootstepState, FootstepGraph>::Ptr node, FootstepGraph::Ptr graph);
00280   double footstepHeuristicStraightRotation(
00281     SolverNode<FootstepState, FootstepGraph>::Ptr node, FootstepGraph::Ptr graph);
00282   double footstepHeuristicStepCost(
00283     SolverNode<FootstepState, FootstepGraph>::Ptr node, FootstepGraph::Ptr graph,
00284     double first_rotation_weight,
00285     double second_rotation_weight);
00286   double footstepHeuristicFollowPathLine(
00287     SolverNode<FootstepState, FootstepGraph>::Ptr node, FootstepGraph::Ptr graph);
00288 }
00289 
00290 #endif


jsk_footstep_planner
Author(s): Ryohei Ueda
autogenerated on Wed Jul 19 2017 02:54:28