00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
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 {
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
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;
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
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));
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
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
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
00262 TransitionLimit::Ptr transition_limit_;
00263 TransitionLimit::Ptr global_transition_limit_;
00264 FootstepParameters parameters_;
00265
00266 ros::WallDuration perception_duration_;
00267
00268
00269 jsk_recognition_utils::PolyLine::Ptr heuristic_path_;
00270 private:
00271 SuccessorFunction successor_func_;
00272 PathCostFunction path_cost_func_;
00273 };
00274
00275
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