footstep_graph.h
Go to the documentation of this file.
1 // -*- mode: c++ -*-
2 /*********************************************************************
3  * Software License Agreement (BSD License)
4  *
5  * Copyright (c) 2015, JSK Lab
6  * All rights reserved.
7  *
8  * Redistribution and use in source and binary forms, with or without
9  * modification, are permitted provided that the following conditions
10  * are met:
11  *
12  * * Redistributions of source code must retain the above copyright
13  * notice, this list of conditions and the following disclaimer.
14  * * Redistributions in binary form must reproduce the above
15  * copyright notice, this list of conditions and the following
16  * disclaimer in the documentation and/o2r other materials provided
17  * with the distribution.
18  * * Neither the name of the JSK Lab nor the names of its
19  * contributors may be used to endorse or promote products derived
20  * from this software without specific prior written permission.
21  *
22  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
23  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
24  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
25  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
26  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
27  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
28  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
29  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
30  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
31  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
32  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
33  * POSSIBILITY OF SUCH DAMAGE.
34  *********************************************************************/
35 
36 
37 #ifndef JSK_FOOTSTEP_PLANNER_FOOTSTEP_GRAPH_H_
38 #define JSK_FOOTSTEP_PLANNER_FOOTSTEP_GRAPH_H_
39 
40 #include <ros/ros.h>
41 #include <pcl_ros/point_cloud.h>
42 #include <jsk_footstep_msgs/FootstepArray.h>
43 
51 
52 namespace jsk_footstep_planner
53 {
54  class FootstepGraph: public Graph<FootstepState>
55  {
56  public:
58  typedef typename boost::function< bool(StatePtr target_state, std::vector<StatePtr> &) > SuccessorFunction;
59  typedef typename boost::function< double(StatePtr, StatePtr, double) > PathCostFunction;
60 
63  FootstepGraph(const Eigen::Vector3f& resolution,
64  const bool use_pointcloud_model = false,
65  const bool lazy_projection = true,
66  const bool local_movement = false,
67  const bool use_obstacle_model = false):
69  publish_progress_(false),
70  resolution_(resolution),
71  use_pointcloud_model_(use_pointcloud_model),
72  lazy_projection_(lazy_projection),
73  local_movement_(local_movement),
74  use_obstacle_model_(use_obstacle_model),
75  pointcloud_model_2d_(new pcl::PointCloud<pcl::PointNormal>),
76  tree_model_(new pcl::KdTreeFLANN<pcl::PointNormal>),
77  obstacle_tree_model_(new pcl::KdTreeFLANN<pcl::PointXYZ>),
78  tree_model_2d_(new pcl::search::Octree<pcl::PointNormal>(0.2)),
79  grid_search_(new ANNGrid(0.05)),
80  parameters_(),
82  Eigen::Affine3f::Identity(),
83  Eigen::Vector3f::UnitX(),
84  resolution_)),
86  {
87  }
88  virtual std::vector<StatePtr> successors(StatePtr target_state) {
89  std::vector<StatePtr> ret;
90  successor_func_(target_state, ret);
91  return ret;
92  }
93 
94  virtual bool isGoal(StatePtr state);
95 
96  virtual double pathCost(StatePtr from, StatePtr to, double prev_cost)
97  {
98  return path_cost_func_(from, to, prev_cost);
99  }
100 
105  virtual bool isColliding(StatePtr current_state, StatePtr previous_state);
106  virtual pcl::PointIndices::Ptr getPointIndicesCollidingSphere(const Eigen::Affine3f& c);
107  virtual bool isCollidingBox(const Eigen::Affine3f& c, pcl::PointIndices::Ptr candidates) const;
113  virtual Eigen::Affine3f getRobotCoords(StatePtr current_state, StatePtr previous_state) const;
114  virtual void setBasicSuccessors(
115  std::vector<Eigen::Affine3f> left_to_right_successors);
116 
117  virtual void setGoalState(
119  {
120  left_goal_state_ = left;
122  }
123 
125  {
126  left_goal_state_ = goal;
127  }
128 
130  {
131  right_goal_state_ = goal;
132  }
133 
138  virtual std::string infoString() const;
139 
140  bool finalizeSteps(const StatePtr &last_1_Step, const StatePtr &lastStep,
141  std::vector<StatePtr> &finalizeSteps);
142 
143  virtual FootstepState::Ptr getGoal(int leg)
144  {
145  if (leg == jsk_footstep_msgs::Footstep::LEFT) {
146  return left_goal_state_;
147  }
148  else if (leg == jsk_footstep_msgs::Footstep::RIGHT) {
149  return right_goal_state_;
150  }
151  else { // TODO: error
152  return goal_state_;
153  }
154  }
155 
156  virtual double maxSuccessorDistance()
157  {
159  }
160  virtual double maxSuccessorRotation()
161  {
163  }
164 
165  virtual void setProgressPublisher(ros::NodeHandle& nh, std::string topic)
166  {
167  publish_progress_ = true;
168  pub_progress_ = nh.advertise<jsk_footstep_msgs::FootstepArray>(topic, 1);
169  }
170 
171  virtual void setPointCloudModel(pcl::PointCloud<pcl::PointNormal>::Ptr model)
172  {
174  tree_model_->setInputCloud(pointcloud_model_);
175  // Project point_cloud_model_
176  pcl::ProjectInliers<pcl::PointNormal> proj;
177  proj.setModelType(pcl::SACMODEL_PLANE);
178  pcl::ModelCoefficients::Ptr
179  plane_coefficients (new pcl::ModelCoefficients);
180  plane_coefficients->values.resize(4.0);
181  plane_coefficients->values[2] = 1.0; // TODO: configurable
182  proj.setModelCoefficients(plane_coefficients);
183  proj.setInputCloud(pointcloud_model_);
184  proj.filter(*pointcloud_model_2d_);
185  tree_model_2d_->setInputCloud(pointcloud_model_2d_);
186  grid_search_->build(*model);
187  }
188 
189  virtual void setObstacleModel(pcl::PointCloud<pcl::PointXYZ>::Ptr model)
190  {
192  obstacle_tree_model_->setInputCloud(obstacle_model_);
193  }
194 
195  virtual bool projectGoal();
196  virtual bool projectStart();
197  virtual bool isSuccessable(StatePtr current_state, StatePtr previous_state);
198  virtual bool useObstacleModel() const { return use_obstacle_model_; }
199  virtual bool usePointCloudModel() const { return use_pointcloud_model_; }
200  virtual bool lazyProjection() const { return lazy_projection_; }
201  virtual bool localMovement() const { return local_movement_; }
202 
209  virtual FootstepState::Ptr projectFootstep(FootstepState::Ptr in, unsigned int& state);
212  virtual std::vector<FootstepState::Ptr> localMoveFootstepState(FootstepState::Ptr in);
213  virtual void setCollisionBBoxOffset(const Eigen::Affine3f& offset) { collision_bbox_offset_ = offset; }
214  virtual void setCollisionBBoxSize(const Eigen::Vector3f& size) { collision_bbox_size_ = size; }
215 
217  successor_func_ = s;
218  }
219  virtual void setPathCostFunction(PathCostFunction p) {
220  path_cost_func_ = p;
221  }
222  bool successors_original(StatePtr target_state, std::vector<FootstepGraph::StatePtr> &ret);
223  double path_cost_original(StatePtr from, StatePtr to, double prev_cost) {
224  // path cost is a number of steps
225  return prev_cost + 1;
226  }
228  {
229  heuristic_path_.reset(new jsk_recognition_utils::PolyLine(path_line)); // copy ???
230  }
231  protected:
232  pcl::PointCloud<pcl::PointNormal>::Ptr pointcloud_model_;
233  pcl::PointCloud<pcl::PointXYZ>::Ptr obstacle_model_;
234  pcl::PointCloud<pcl::PointNormal>::Ptr pointcloud_model_2d_;
235  pcl::KdTreeFLANN<pcl::PointNormal>::Ptr tree_model_;
236  pcl::KdTreeFLANN<pcl::PointXYZ>::Ptr obstacle_tree_model_;
237  //pcl::KdTreeFLANN<pcl::PointNormal>::Ptr tree_model_2d_;
238  pcl::search::Octree<pcl::PointNormal>::Ptr tree_model_2d_;
240  std::vector<Eigen::Affine3f> successors_from_left_to_right_;
241  std::vector<Eigen::Affine3f> successors_from_right_to_left_;
249  Eigen::Affine3f collision_bbox_offset_;
250  Eigen::Vector3f collision_bbox_size_;
255  // const params
257  const bool lazy_projection_;
258  const bool local_movement_;
260  const Eigen::Vector3f resolution_;
261  // params
265 
267 
268  // add hint for huristic
270  private:
272  PathCostFunction path_cost_func_;
273  };
274 
275  // heuristic function
276  double footstepHeuristicZero(
284  double first_rotation_weight,
285  double second_rotation_weight);
288 }
289 
290 #endif
ANNGrid is a class to provide approximate near neighbors search based on 2.5-D representation. All the z values of pointcloud is ignored and it sorted as 2-D array.
Definition: ann_grid.h:93
double footstepHeuristicStepCost(SolverNode< FootstepState, FootstepGraph >::Ptr node, FootstepGraph::Ptr graph, double first_rotation_weight, double second_rotation_weight)
pcl::PointCloud< pcl::PointXYZ >::Ptr obstacle_model_
virtual bool isColliding(StatePtr current_state, StatePtr previous_state)
return True if current_state collides with obstacle.
virtual bool lazyProjection() const
virtual bool isGoal(StatePtr state)
void setHeuristicPathLine(jsk_recognition_utils::PolyLine &path_line)
virtual bool localMovement() const
virtual void setProgressPublisher(ros::NodeHandle &nh, std::string topic)
pcl::search::Octree< pcl::PointNormal >::Ptr tree_model_2d_
std::vector< Eigen::Affine3f > successors_from_left_to_right_
virtual ros::WallDuration getPerceptionDuration()
double footstepHeuristicStraightRotation(SolverNode< FootstepState, FootstepGraph >::Ptr node, FootstepGraph::Ptr graph)
virtual void setCollisionBBoxSize(const Eigen::Vector3f &size)
virtual bool usePointCloudModel() const
virtual void setPointCloudModel(pcl::PointCloud< pcl::PointNormal >::Ptr model)
virtual void setObstacleModel(pcl::PointCloud< pcl::PointXYZ >::Ptr model)
jsk_recognition_utils::PolyLine::Ptr heuristic_path_
pcl::PointCloud< pcl::PointNormal >::Ptr pointcloud_model_2d_
virtual bool isCollidingBox(const Eigen::Affine3f &c, pcl::PointIndices::Ptr candidates) const
friend double footstepHeuristicFollowPathLine(SolverNode< FootstepState, FootstepGraph >::Ptr node, FootstepGraph::Ptr graph)
virtual std::vector< StatePtr > successors(StatePtr target_state)
virtual void setGoalState(FootstepState::Ptr left, FootstepState::Ptr right)
virtual void setGlobalTransitionLimit(TransitionLimit::Ptr limit)
double path_cost_original(StatePtr from, StatePtr to, double prev_cost)
std::string model
double footstepHeuristicStraight(SolverNode< FootstepState, FootstepGraph >::Ptr node, FootstepGraph::Ptr graph)
virtual void setCollisionBBoxOffset(const Eigen::Affine3f &offset)
sensor_msgs::PointCloud2 PointCloud
c
Identity
pcl::KdTreeFLANN< pcl::PointXYZ >::Ptr obstacle_tree_model_
virtual void setPathCostFunction(PathCostFunction p)
virtual Eigen::Affine3f getRobotCoords(StatePtr current_state, StatePtr previous_state) const
Compute robot coords from current footstep and previous footstep. R_robot = midcoords(F_current, F_previous) * R_offset;.
std::vector< Eigen::Affine3f > successors_from_right_to_left_
TransitionLimit::Ptr global_transition_limit_
virtual bool isSuccessable(StatePtr current_state, StatePtr previous_state)
const Eigen::Vector3f resolution(0.05, 0.05, 0.08)
virtual void setParameters(FootstepParameters &p)
state
FootstepGraph::Ptr graph
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
virtual TransitionLimit::Ptr getTransitionLimit()
double footstepHeuristicZero(SolverNode< FootstepState, FootstepGraph >::Ptr node, FootstepGraph::Ptr graph)
virtual void setRightGoalState(FootstepState::Ptr goal)
virtual void setBasicSuccessors(std::vector< Eigen::Affine3f > left_to_right_successors)
boost::function< double(StatePtr, StatePtr, double) > PathCostFunction
bool finalizeSteps(const StatePtr &last_1_Step, const StatePtr &lastStep, std::vector< StatePtr > &finalizeSteps)
boost::function< bool(StatePtr target_state, std::vector< StatePtr > &) > SuccessorFunction
FootstepGraph(const Eigen::Vector3f &resolution, const bool use_pointcloud_model=false, const bool lazy_projection=true, const bool local_movement=false, const bool use_obstacle_model=false)
FootstepState::Ptr zero_state_
zero_state is used only for global transition limit
virtual void setTransitionLimit(TransitionLimit::Ptr limit)
virtual TransitionLimit::Ptr getGlobalTransitionLimit()
bool successors_original(StatePtr target_state, std::vector< FootstepGraph::StatePtr > &ret)
virtual pcl::PointIndices::Ptr getPointIndicesCollidingSphere(const Eigen::Affine3f &c)
pcl::KdTreeFLANN< pcl::PointNormal >::Ptr tree_model_
virtual FootstepState::Ptr getGoal(int leg)
pcl::PointCloud< pcl::PointNormal >::Ptr pointcloud_model_
virtual double pathCost(StatePtr from, StatePtr to, double prev_cost)
boost::shared_ptr< FootstepGraph > Ptr
virtual FootstepState::Ptr projectFootstep(FootstepState::Ptr in)
TransitionLimit::Ptr transition_limit_
ROSCPP_DECL bool search(const std::string &ns, const std::string &key, std::string &result)
virtual std::vector< FootstepState::Ptr > localMoveFootstepState(FootstepState::Ptr in)
virtual std::string infoString() const
return string about graph information.
virtual void setSuccessorFunction(SuccessorFunction s)
virtual void setLeftGoalState(FootstepState::Ptr goal)
virtual bool useObstacleModel() const


jsk_footstep_planner
Author(s): Ryohei Ueda
autogenerated on Thu Nov 14 2019 03:53:28