footstep_planner.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_PLANNER_H_
38 #define JSK_FOOTSTEP_PLANNER_FOOTSTEP_PLANNER_H_
39 
40 #include <ros/ros.h>
42 
43 // ros
44 #include <jsk_footstep_msgs/FootstepArray.h>
45 #include <jsk_footstep_msgs/PlanFootstepsAction.h>
46 #include <jsk_footstep_planner/FootstepPlannerConfig.h>
47 #include <sensor_msgs/PointCloud2.h>
48 #include <dynamic_reconfigure/server.h>
49 #include <jsk_rviz_plugins/OverlayText.h>
50 
51 // footstep planning
56 #include <jsk_footstep_planner/CollisionBoundingBoxInfo.h>
57 #include <jsk_interactive_marker/SnapFootPrint.h>
58 
59 #include "jsk_footstep_planner/ProjectFootstep.h"
60 #include "jsk_footstep_planner/SetHeuristicPath.h"
61 
62 namespace jsk_footstep_planner
63 {
65  {
67  };
73  {
74  public:
76  typedef FootstepPlannerConfig Config;
79 
80  protected:
81 
82  // Initialization
83  virtual bool readSuccessors(ros::NodeHandle& nh);
84 
85  virtual void pointcloudCallback(const sensor_msgs::PointCloud2::ConstPtr& msg);
86  virtual void obstacleCallback(const sensor_msgs::PointCloud2::ConstPtr& msg);
87  virtual void planCB(const jsk_footstep_msgs::PlanFootstepsGoal::ConstPtr& goal);
88 
89  // buildGraph is not thread safe, it is responsible for caller to take care
90  // of mutex
91  virtual void buildGraph();
92 
93  virtual void configCallback(Config &config, uint32_t level);
94 
95  virtual double stepCostHeuristic(
97  virtual double zeroHeuristic(
99  virtual double straightHeuristic(
101  virtual double straightRotationHeuristic(
103  virtual double followPathLineHeuristic(
106  virtual void publishPointCloud(
107  const pcl::PointCloud<pcl::PointNormal>& cloud,
109  const std_msgs::Header& header);
110  virtual bool projectFootPrint(
111  const Eigen::Affine3f& center_pose,
112  const Eigen::Affine3f& left_pose_trans,
113  const Eigen::Affine3f& right_pose_trans,
114  geometry_msgs::Pose& pose);
115 
116  virtual bool projectFootPrintService(
117  jsk_interactive_marker::SnapFootPrint::Request& req,
118  jsk_interactive_marker::SnapFootPrint::Response& res);
119  virtual bool projectFootstepService(
120  jsk_footstep_planner::ProjectFootstep::Request& req,
121  jsk_footstep_planner::ProjectFootstep::Response& res);
122  virtual bool collisionBoundingBoxInfoService(
123  jsk_footstep_planner::CollisionBoundingBoxInfo::Request& req,
124  jsk_footstep_planner::CollisionBoundingBoxInfo::Response& res);
126  jsk_interactive_marker::SnapFootPrint::Request& req,
127  jsk_interactive_marker::SnapFootPrint::Response& res);
128  virtual bool setHeuristicPathService(
129  jsk_footstep_planner::SetHeuristicPath::Request& req,
130  jsk_footstep_planner::SetHeuristicPath::Response& res);
131  virtual void publishText(ros::Publisher& pub,
132  const std::string& text,
133  PlanningStatus status);
134 
137  jsk_footstep_msgs::PlanFootstepsResult result_;
149  pcl::PointCloud<pcl::PointNormal>::Ptr pointcloud_model_;
150  pcl::PointCloud<pcl::PointXYZ>::Ptr obstacle_model_;
152  std::vector<Eigen::Affine3f> successors_;
153  Eigen::Vector3f collision_bbox_size_;
154  Eigen::Affine3f collision_bbox_offset_;
155  Eigen::Vector3f inv_lleg_footstep_offset_;
156  Eigen::Vector3f inv_rleg_footstep_offset_;
157  std_msgs::Header latest_header_;
158  // Common Parameters
160 
161  // Parameters
178  std::string heuristic_;
181  double cost_weight_;
186 
187  private:
188 
189  };
190 }
191 
192 #endif
pcl::PointCloud< pcl::PointXYZ >::Ptr obstacle_model_
Actionlib server for footstep planning.
virtual void obstacleCallback(const sensor_msgs::PointCloud2::ConstPtr &msg)
virtual bool projectFootstepService(jsk_footstep_planner::ProjectFootstep::Request &req, jsk_footstep_planner::ProjectFootstep::Response &res)
virtual double stepCostHeuristic(SolverNode< FootstepState, FootstepGraph >::Ptr node, FootstepGraph::Ptr graph)
virtual void configCallback(Config &config, uint32_t level)
virtual void profile(FootstepAStarSolver< FootstepGraph > &solver, FootstepGraph::Ptr graph)
virtual bool projectFootPrintService(jsk_interactive_marker::SnapFootPrint::Request &req, jsk_interactive_marker::SnapFootPrint::Response &res)
pcl::PointCloud< pcl::PointNormal >::Ptr pointcloud_model_
virtual bool projectFootPrintWithLocalSearchService(jsk_interactive_marker::SnapFootPrint::Request &req, jsk_interactive_marker::SnapFootPrint::Response &res)
virtual double zeroHeuristic(SolverNode< FootstepState, FootstepGraph >::Ptr node, FootstepGraph::Ptr graph)
virtual bool projectFootPrint(const Eigen::Affine3f &center_pose, const Eigen::Affine3f &left_pose_trans, const Eigen::Affine3f &right_pose_trans, geometry_msgs::Pose &pose)
ros::ServiceServer srv_project_footprint_with_local_search_
boost::shared_ptr< FootstepPlanner > Ptr
pcl::PointCloud< pcl::PointNormal >::Ptr cloud
FootstepGraph::Ptr graph
virtual void publishPointCloud(const pcl::PointCloud< pcl::PointNormal > &cloud, ros::Publisher &pub, const std_msgs::Header &header)
boost::shared_ptr< dynamic_reconfigure::Server< Config > > srv_
virtual bool collisionBoundingBoxInfoService(jsk_footstep_planner::CollisionBoundingBoxInfo::Request &req, jsk_footstep_planner::CollisionBoundingBoxInfo::Response &res)
virtual bool setHeuristicPathService(jsk_footstep_planner::SetHeuristicPath::Request &req, jsk_footstep_planner::SetHeuristicPath::Response &res)
virtual double straightHeuristic(SolverNode< FootstepState, FootstepGraph >::Ptr node, FootstepGraph::Ptr graph)
ros::ServiceServer srv_collision_bounding_box_info_
std::vector< Eigen::Affine3f > successors_
virtual bool readSuccessors(ros::NodeHandle &nh)
actionlib::SimpleActionServer< jsk_footstep_msgs::PlanFootstepsAction > as_
virtual void planCB(const jsk_footstep_msgs::PlanFootstepsGoal::ConstPtr &goal)
boost::mutex mutex
virtual void publishText(ros::Publisher &pub, const std::string &text, PlanningStatus status)
virtual double followPathLineHeuristic(SolverNode< FootstepState, FootstepGraph >::Ptr node, FootstepGraph::Ptr graph)
virtual void pointcloudCallback(const sensor_msgs::PointCloud2::ConstPtr &msg)
virtual double straightRotationHeuristic(SolverNode< FootstepState, FootstepGraph >::Ptr node, FootstepGraph::Ptr graph)
virtual void setHeuristicPathLine(jsk_recognition_utils::PolyLine &path_line)
jsk_footstep_msgs::PlanFootstepsResult result_


jsk_footstep_planner
Author(s): Ryohei Ueda
autogenerated on Fri May 14 2021 02:51:52