footstep_planner.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_PLANNER_H_
00038 #define JSK_FOOTSTEP_PLANNER_FOOTSTEP_PLANNER_H_
00039 
00040 #include <ros/ros.h>
00041 #include <actionlib/server/simple_action_server.h>
00042 
00043 // ros
00044 #include <jsk_footstep_msgs/FootstepArray.h>
00045 #include <jsk_footstep_msgs/PlanFootstepsAction.h>
00046 #include <jsk_footstep_planner/FootstepPlannerConfig.h>
00047 #include <sensor_msgs/PointCloud2.h>
00048 #include <dynamic_reconfigure/server.h>
00049 #include <jsk_rviz_plugins/OverlayText.h>
00050 
00051 // footstep planning
00052 #include "jsk_footstep_planner/footstep_graph.h"
00053 #include "jsk_footstep_planner/astar_solver.h"
00054 #include "jsk_footstep_planner/footstep_astar_solver.h"
00055 #include "jsk_footstep_planner/footstep_parameters.h"
00056 #include <jsk_footstep_planner/CollisionBoundingBoxInfo.h>
00057 #include <jsk_interactive_marker/SnapFootPrint.h>
00058 
00059 #include "jsk_footstep_planner/ProjectFootstep.h"
00060 #include "jsk_footstep_planner/SetHeuristicPath.h"
00061 
00062 namespace jsk_footstep_planner
00063 {
00064   enum PlanningStatus
00065   {
00066     OK, WARNING, ERROR
00067   };
00072   class FootstepPlanner
00073   {
00074   public:
00075     typedef boost::shared_ptr<FootstepPlanner> Ptr;
00076     typedef FootstepPlannerConfig Config;
00077     FootstepPlanner(ros::NodeHandle& nh);
00078     virtual void setHeuristicPathLine(jsk_recognition_utils::PolyLine &path_line);
00079 
00080   protected:
00081 
00082     // Initialization
00083     virtual bool readSuccessors(ros::NodeHandle& nh);
00084     
00085     virtual void pointcloudCallback(const sensor_msgs::PointCloud2::ConstPtr& msg);
00086     virtual void obstacleCallback(const sensor_msgs::PointCloud2::ConstPtr& msg);
00087     virtual void planCB(const jsk_footstep_msgs::PlanFootstepsGoal::ConstPtr& goal);
00088 
00089     // buildGraph is not thread safe, it is responsible for caller to take care
00090     // of mutex
00091     virtual void buildGraph();
00092 
00093     virtual void configCallback(Config &config, uint32_t level);
00094 
00095     virtual double stepCostHeuristic(
00096       SolverNode<FootstepState, FootstepGraph>::Ptr node, FootstepGraph::Ptr graph);
00097     virtual double zeroHeuristic(
00098       SolverNode<FootstepState, FootstepGraph>::Ptr node, FootstepGraph::Ptr graph);
00099     virtual double straightHeuristic(
00100       SolverNode<FootstepState, FootstepGraph>::Ptr node, FootstepGraph::Ptr graph);
00101     virtual double straightRotationHeuristic(
00102       SolverNode<FootstepState, FootstepGraph>::Ptr node, FootstepGraph::Ptr graph);
00103     virtual double followPathLineHeuristic(
00104       SolverNode<FootstepState, FootstepGraph>::Ptr node, FootstepGraph::Ptr graph);
00105     virtual void profile(FootstepAStarSolver<FootstepGraph>& solver, FootstepGraph::Ptr graph);
00106     virtual void publishPointCloud(
00107       const pcl::PointCloud<pcl::PointNormal>& cloud,
00108       ros::Publisher& pub,
00109       const std_msgs::Header& header);
00110     virtual bool projectFootPrint(
00111       const Eigen::Affine3f& center_pose,
00112       const Eigen::Affine3f& left_pose_trans,
00113       const Eigen::Affine3f& right_pose_trans,
00114       geometry_msgs::Pose& pose);
00115 
00116     virtual bool projectFootPrintService(
00117       jsk_interactive_marker::SnapFootPrint::Request& req,
00118       jsk_interactive_marker::SnapFootPrint::Response& res);
00119     virtual bool projectFootstepService(
00120       jsk_footstep_planner::ProjectFootstep::Request& req,
00121       jsk_footstep_planner::ProjectFootstep::Response& res);
00122     virtual bool collisionBoundingBoxInfoService(
00123       jsk_footstep_planner::CollisionBoundingBoxInfo::Request& req,
00124       jsk_footstep_planner::CollisionBoundingBoxInfo::Response& res);
00125     virtual bool projectFootPrintWithLocalSearchService(
00126       jsk_interactive_marker::SnapFootPrint::Request& req,
00127       jsk_interactive_marker::SnapFootPrint::Response& res);
00128     virtual bool setHeuristicPathService(
00129       jsk_footstep_planner::SetHeuristicPath::Request& req,
00130       jsk_footstep_planner::SetHeuristicPath::Response& res);
00131     virtual void publishText(ros::Publisher& pub,
00132                              const std::string& text,
00133                              PlanningStatus status);
00134 
00135     boost::mutex mutex_;
00136     actionlib::SimpleActionServer<jsk_footstep_msgs::PlanFootstepsAction> as_;
00137     jsk_footstep_msgs::PlanFootstepsResult result_;
00138     boost::shared_ptr <dynamic_reconfigure::Server<Config> > srv_;
00139     ros::Publisher pub_close_list_;
00140     ros::Publisher pub_open_list_;
00141     ros::Publisher pub_text_;
00142     ros::Subscriber sub_pointcloud_model_;
00143     ros::Subscriber sub_obstacle_model_;
00144     ros::ServiceServer srv_project_footprint_;
00145     ros::ServiceServer srv_project_footprint_with_local_search_;
00146     ros::ServiceServer srv_collision_bounding_box_info_;
00147     ros::ServiceServer srv_project_footstep_;
00148     ros::ServiceServer srv_set_heuristic_path_;
00149     pcl::PointCloud<pcl::PointNormal>::Ptr pointcloud_model_;
00150     pcl::PointCloud<pcl::PointXYZ>::Ptr obstacle_model_;
00151     FootstepGraph::Ptr graph_;
00152     std::vector<Eigen::Affine3f> successors_;
00153     Eigen::Vector3f collision_bbox_size_;
00154     Eigen::Affine3f collision_bbox_offset_;
00155     Eigen::Vector3f inv_lleg_footstep_offset_;
00156     Eigen::Vector3f inv_rleg_footstep_offset_;
00157     std_msgs::Header latest_header_;
00158     // Common Parameters
00159     FootstepParameters parameters_;
00160 
00161     // Parameters
00162     bool rich_profiling_;
00163     bool project_start_state_;
00164     bool project_goal_state_;
00165     bool use_pointcloud_model_;
00166     bool use_lazy_perception_;
00167     bool use_local_movement_;
00168     bool use_obstacle_model_;
00169     double resolution_x_;
00170     double resolution_y_;
00171     double resolution_theta_;
00172     double footstep_size_x_;
00173     double footstep_size_y_;
00174     int close_list_x_num_;
00175     int close_list_y_num_;
00176     int close_list_theta_num_;
00177     int profile_period_;
00178     std::string heuristic_;
00179     double heuristic_first_rotation_weight_;
00180     double heuristic_second_rotation_weight_;
00181     double cost_weight_;
00182     double heuristic_weight_;
00183     std::string pointcloud_model_frame_id_, obstacle_model_frame_id_;
00184     double planning_timeout_;
00185 
00186   private:
00187     
00188   };
00189 }
00190 
00191 #endif


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