grid_path_planner.h
Go to the documentation of this file.
00001 // -*- mode: c++ -*-
00002 
00003 #ifndef JSK_FOOTSTEP_PLANNER_GRID_PATH_PLANNER_H_
00004 #define JSK_FOOTSTEP_PLANNER_GRID_PATH_PLANNER_H_
00005 
00006 #include <ros/ros.h>
00007 #include <actionlib/server/simple_action_server.h>
00008 
00009 // ros
00010 #include <sensor_msgs/PointCloud2.h>
00011 //#include <dynamic_reconfigure/server.h>
00012 #include <jsk_rviz_plugins/OverlayText.h>
00013 #include <jsk_footstep_msgs/FootstepArray.h>
00014 #include <jsk_footstep_msgs/PlanFootstepsAction.h>
00015 //#include <jsk_footstep_planner/FootstepPlannerConfig.h>
00016 #include <jsk_footstep_planner/CollisionBoundingBoxInfo.h>
00017 
00018 // pcl
00019 #include <pcl/kdtree/kdtree_flann.h>
00020 
00021 // grid path planning
00022 #include "jsk_footstep_planner/grid_perception.h"
00023 
00024 namespace jsk_footstep_planner
00025 {
00026   enum GridPlanningStatus
00027   {
00028     NONE,
00029     OK,
00030     WARNING,
00031     ERROR
00032   };
00037   class GridPathPlanner
00038   {
00039   public:
00040     typedef PerceptionGridGraph Graph;
00041     typedef PerceptionGridMap GridMap;
00042     typedef GridAStarSolver<PerceptionGridGraph> Solver;
00043 
00044     GridPathPlanner(ros::NodeHandle& nh);
00045 
00046   protected:
00047     virtual void pointcloudCallback(const sensor_msgs::PointCloud2::ConstPtr& msg);
00048     virtual void obstacleCallback(const sensor_msgs::PointCloud2::ConstPtr& msg);
00049     virtual void planCB(const jsk_footstep_msgs::PlanFootstepsGoal::ConstPtr& goal);
00050 
00051     virtual bool collisionBoundingBoxInfoService(
00052       jsk_footstep_planner::CollisionBoundingBoxInfo::Request& req,
00053       jsk_footstep_planner::CollisionBoundingBoxInfo::Response& res);
00054     // buildGraph is not thread safe, it is responsible for caller to take care
00055     // of mutex
00056     virtual void buildGraph();
00057 
00058     //    virtual void profile(FootstepAStarSolver<FootstepGraph>& solver, FootstepGraph::Ptr graph);
00059 
00060     virtual void publishPointCloud(
00061       const pcl::PointCloud<pcl::PointXYZRGB>& cloud,
00062       ros::Publisher& pub,
00063       const std_msgs::Header& header);
00064 
00065     virtual void publishText(ros::Publisher& pub,
00066                              const std::string& text,
00067                              GridPlanningStatus status);
00068 
00069     virtual bool updateCost(GridState::Ptr ptr);
00070     //solver.openListToPointCloud(open_list_cloud);
00071     //solver.closeListToPointCloud(close_list_cloud);
00072 
00073     virtual void publishMarker();
00074 
00075     inline virtual void gridToPoint(const int ix, const int iy, Eigen::Vector3f &p)
00076     {
00077       Eigen::Vector3f add(map_resolution_ * (ix + 0.5), map_resolution_ * (iy + 0.5), 0);
00078       p = map_offset_ + add;
00079     }
00080 
00081     inline virtual void pointToGrid(const Eigen::Vector3f &p, int &ix, int &iy)
00082     {
00083       Eigen::Vector3f ipos = p - map_offset_;
00084       ix = (int)(ipos[0] / map_resolution_);
00085       iy = (int)(ipos[1] / map_resolution_);
00086     }
00087     // profile
00088 
00089     double heuristicDistance(SolverNode<PerceptionGridGraph::State,
00090                              PerceptionGridGraph>::Ptr node,
00091                              PerceptionGridGraph::Ptr graph) {
00092       int ix = node->getState()->indexX();
00093       int iy = node->getState()->indexY();
00094       double gx = graph->getGoalState()->indexX() - ix;
00095       double gy = graph->getGoalState()->indexY() - iy;
00096 
00097       return std::sqrt(gx * gx + gy * gy);
00098     }
00099 
00100     boost::mutex mutex_;
00101     actionlib::SimpleActionServer<jsk_footstep_msgs::PlanFootstepsAction> as_;
00102     jsk_footstep_msgs::PlanFootstepsResult result_;
00103 
00104     ros::Publisher pub_close_list_;
00105     ros::Publisher pub_open_list_;
00106     ros::Publisher pub_text_;
00107     ros::Publisher pub_marker_;
00108 
00109     ros::Subscriber sub_plane_points_; // plane for stepping
00110     ros::Subscriber sub_obstacle_points_;   // collision detection with environment
00111 
00112     ros::ServiceServer srv_collision_bounding_box_info_;
00113 
00114     pcl::PointCloud<pcl::PointNormal>::Ptr plane_points_;
00115     pcl::PointCloud<pcl::PointXYZ>::Ptr    obstacle_points_;
00116     pcl::KdTreeFLANN<pcl::PointNormal>::Ptr plane_tree_;
00117     pcl::KdTreeFLANN<pcl::PointXYZ>::Ptr    obstacle_tree_;
00118 
00119     std::string plane_points_frame_id_;
00120     std::string obstacle_points_frame_id_;
00121 
00122     PerceptionGridMap::Ptr gridmap_;
00123     PerceptionGridGraph::Ptr graph_;
00124 
00125     Eigen::Vector3f map_offset_;
00126 
00127     // Parameters
00128     Eigen::Vector3f collision_bbox_size_;
00129     Eigen::Affine3f collision_bbox_offset_;
00130 
00131     double map_resolution_;
00132     double collision_circle_radius_;
00133     double collision_circle_min_height_;
00134     double collision_circle_max_height_;
00135 
00136     bool use_obstacle_points_;
00137     bool use_plane_points_;
00138   private:
00139   };
00140 }
00141 
00142 #endif


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