grid_path_planner.cpp
Go to the documentation of this file.
00001 // -*- mode: c++ -*-
00002 
00003 #include "jsk_footstep_planner/grid_path_planner.h"
00004 
00005 //param
00006 #include <jsk_topic_tools/rosparam_utils.h>
00007 
00008 #include <jsk_recognition_utils/pcl_conversion_util.h> // kdtree
00009 
00010 #include <visualization_msgs/MarkerArray.h>
00011 #include <jsk_recognition_utils/geo/polyline.h>
00012 
00013 namespace jsk_footstep_planner
00014 {
00015   GridPathPlanner::GridPathPlanner(ros::NodeHandle& nh):
00016     as_(nh, nh.getNamespace(),
00017         boost::bind(&GridPathPlanner::planCB, this, _1), false),
00018     plane_tree_(new pcl::KdTreeFLANN<pcl::PointNormal>),
00019     obstacle_tree_(new pcl::KdTreeFLANN<pcl::PointXYZ>)
00020   {
00021     pub_text_ = nh.advertise<jsk_rviz_plugins::OverlayText> ("text", 1, true);
00022     pub_marker_ = nh.advertise<visualization_msgs::MarkerArray> ("grid_graph_marker", 2, true);
00023     pub_close_list_ = nh.advertise<sensor_msgs::PointCloud2> ("close_list", 1, true);
00024     pub_open_list_  = nh.advertise<sensor_msgs::PointCloud2> ("open_list", 1, true);
00025 
00026     srv_collision_bounding_box_info_ = nh.advertiseService(
00027       "collision_bounding_box_info", &GridPathPlanner::collisionBoundingBoxInfoService, this);
00028 
00029     sub_plane_points_ = nh.subscribe("plane_points", 1, &GridPathPlanner::pointcloudCallback, this);
00030     sub_obstacle_points_   = nh.subscribe("obstacle_points", 1, &GridPathPlanner::obstacleCallback, this);
00031 
00032     std::vector<double> collision_bbox_size, collision_bbox_offset;
00033     if (jsk_topic_tools::readVectorParameter(nh, "collision_bbox_size", collision_bbox_size)) {
00034       collision_bbox_size_[0] = collision_bbox_size[0];
00035       collision_bbox_size_[1] = collision_bbox_size[1];
00036       collision_bbox_size_[2] = collision_bbox_size[2];
00037     }
00038     if (jsk_topic_tools::readVectorParameter(nh, "collision_bbox_offset", collision_bbox_offset)) {
00039       collision_bbox_offset_ = Eigen::Affine3f::Identity() * Eigen::Translation3f(collision_bbox_offset[0],
00040                                                                                   collision_bbox_offset[1],
00041                                                                                   collision_bbox_offset[2]);
00042     }
00043 
00044     nh.param("map_resolution", map_resolution_, 0.4);
00045     ROS_INFO("map resolution: %f", map_resolution_);
00046     nh.param("collision_circle_radius", collision_circle_radius_, 0.35);
00047     nh.param("collision_circle_min_height", collision_circle_min_height_, 0.4);
00048     nh.param("collision_circle_max_height", collision_circle_max_height_, 1.9);
00049     use_obstacle_points_ = true;
00050     use_plane_points_ = true;
00051 
00052     as_.start();
00053   }
00054 
00055   void GridPathPlanner::buildGraph()
00056   {
00057     //boost::mutex::scoped_lock lock(mutex_);
00058     ROS_INFO("build plane %d", plane_points_->points.size());
00059     Eigen::Vector4f minpt, maxpt;
00060     pcl::getMinMax3D<pcl::PointNormal> (*plane_points_, minpt, maxpt);
00061 
00062     Eigen::Vector4f len = maxpt - minpt;
00063     map_offset_[0] = minpt[0];
00064     map_offset_[1] = minpt[1];
00065     map_offset_[2] = 0.0;
00066     int sizex = (int)(len[0] / map_resolution_);
00067     int sizey = (int)(len[1] / map_resolution_);
00068 
00069     ROS_INFO("min_point: [%f, %f] / size: %d %d",
00070              map_offset_[0], map_offset_[1], sizex, sizey);
00071 
00072     obstacle_tree_->setInputCloud(obstacle_points_);
00073     plane_tree_->setInputCloud(plane_points_);
00074 
00075     gridmap_.reset(new GridMap(sizex, sizey));
00076     gridmap_->setCostFunction(boost::bind(&GridPathPlanner::updateCost, this, _1));
00077     graph_.reset(new Graph(gridmap_));
00078   }
00079 
00080   void GridPathPlanner::obstacleCallback(const sensor_msgs::PointCloud2::ConstPtr& msg)
00081   {
00082     boost::mutex::scoped_lock lock(mutex_);
00083     //ROS_DEBUG("obstacle points is updated");
00084     ROS_INFO("obstacle points is updated");
00085     obstacle_points_.reset(new pcl::PointCloud<pcl::PointXYZ>);
00086     pcl::fromROSMsg(*msg, *obstacle_points_);
00087     obstacle_points_frame_id_ = msg->header.frame_id; // check frame_id
00088   }
00089 
00090   void GridPathPlanner::pointcloudCallback(const sensor_msgs::PointCloud2::ConstPtr& msg)
00091   {
00092     boost::mutex::scoped_lock lock(mutex_);
00093     //ROS_DEBUG("pointcloud points is updated");
00094     ROS_INFO("pointcloud points is updated");
00095     plane_points_.reset(new pcl::PointCloud<pcl::PointNormal>);
00096     pcl::fromROSMsg(*msg, *plane_points_);
00097     plane_points_frame_id_ = msg->header.frame_id; // check frame_id
00098   }
00099 
00100   bool GridPathPlanner::collisionBoundingBoxInfoService(
00101       jsk_footstep_planner::CollisionBoundingBoxInfo::Request& req,
00102       jsk_footstep_planner::CollisionBoundingBoxInfo::Response& res)
00103   {
00104     boost::mutex::scoped_lock lock(mutex_);
00105     res.box_dimensions.x = collision_bbox_size_[0];
00106     res.box_dimensions.y = collision_bbox_size_[1];
00107     res.box_dimensions.z = collision_bbox_size_[2];
00108     tf::poseEigenToMsg(collision_bbox_offset_, res.box_offset);
00109     return true;
00110   }
00111 
00112   void GridPathPlanner::publishText(ros::Publisher& pub,
00113                                     const std::string& text,
00114                                     GridPlanningStatus status)
00115   {
00116     std_msgs::ColorRGBA ok_color;
00117     ok_color.r = 0.3568627450980392;
00118     ok_color.g = 0.7529411764705882;
00119     ok_color.b = 0.8705882352941177;
00120     ok_color.a = 1.0;
00121     std_msgs::ColorRGBA warn_color;
00122     warn_color.r = 0.9411764705882353;
00123     warn_color.g = 0.6784313725490196;
00124     warn_color.b = 0.3058823529411765;
00125     warn_color.a = 1.0;
00126     std_msgs::ColorRGBA error_color;
00127     error_color.r = 0.8509803921568627;
00128     error_color.g = 0.3254901960784314;
00129     error_color.b = 0.30980392156862746;
00130     error_color.a = 1.0;
00131     std_msgs::ColorRGBA color;
00132     if (status == OK) {
00133       color = ok_color;
00134     }
00135     else if (status == WARNING) {
00136       color = warn_color;
00137     }
00138     else if (status == ERROR) {
00139       color = error_color;
00140     }
00141     jsk_rviz_plugins::OverlayText msg;
00142     msg.text = text;
00143     msg.width = 1000;
00144     msg.height = 1000;
00145     msg.top = 10;
00146     msg.left = 10;
00147     msg.bg_color.a = 0.0;
00148     msg.fg_color = color;
00149     msg.text_size = 24;
00150     pub.publish(msg);
00151   }
00152 
00153   void GridPathPlanner::planCB(const jsk_footstep_msgs::PlanFootstepsGoal::ConstPtr& goal)
00154   {
00155     boost::mutex::scoped_lock lock(mutex_);
00156     // latest_header_ = goal->goal_footstep.header;
00157     ROS_INFO("planCB");
00158 
00159     // check frame_id sanity
00160     std::string goal_frame_id = goal->initial_footstep.header.frame_id;
00161 #if 0
00162     if (use_plane_points_) {
00163       // check perception cloud header
00164       if (goal_frame_id != plane_points_frame_id_) {
00165         ROS_ERROR("frame_id of goal and pointcloud do not match. goal: %s, pointcloud: %s.",
00166                       goal_frame_id.c_str(), plane_points_frame_id_.c_str());
00167         as_.setPreempted();
00168         return;
00169       }
00170     }
00171     if (use_obstacle_points_) {
00172       // check perception cloud header
00173       if (goal_frame_id != obstacle_points_frame_id_) {
00174         ROS_ERROR("frame_id of goal and obstacle pointcloud do not match. goal: %s, obstacle: %s.",
00175                       goal_frame_id.c_str(), obstacle_points_frame_id_.c_str());
00176         as_.setPreempted();
00177         return;
00178       }
00179     }
00180 #endif
00181     double initx = 0;
00182     double inity = 0;
00183     double goalx = 0;
00184     double goaly = 0;
00185     for(int i = 0; i < goal->initial_footstep.footsteps.size(); i++) {
00186       initx += goal->initial_footstep.footsteps[i].pose.position.x;
00187       inity += goal->initial_footstep.footsteps[i].pose.position.y;
00188     }
00189     initx /= goal->initial_footstep.footsteps.size();
00190     inity /= goal->initial_footstep.footsteps.size();
00191     for(int i = 0; i < goal->goal_footstep.footsteps.size(); i++) {
00192       goalx += goal->goal_footstep.footsteps[i].pose.position.x;
00193       goaly += goal->goal_footstep.footsteps[i].pose.position.y;
00194     }
00195     goalx /= goal->initial_footstep.footsteps.size();
00196     goaly /= goal->initial_footstep.footsteps.size();
00197 
00198     ROS_INFO("start: %f %f, goal %f %f", initx, inity, goalx, goaly);
00199 
00200     ROS_INFO("build graph");
00201     buildGraph();
00202 
00203     ROS_INFO("solve");
00204     Eigen::Vector3f startp(initx, inity, 0);
00205     Eigen::Vector3f goalp(goalx, goaly, 0);
00206     int sx, sy, gx, gy;
00207     pointToGrid(startp, sx, sy);
00208     pointToGrid(goalp, gx, gy);
00209 
00210     if(!gridmap_->inRange(sx, sy)) {
00211       ROS_ERROR("start is not in range %d %d", sx, sy);
00212       as_.setPreempted();
00213       return;
00214     }
00215 
00216     if(!gridmap_->inRange(gx, gy)) {
00217       ROS_ERROR("goal is not in range %d %d", gx, gy);
00218       as_.setPreempted();
00219       return;
00220     }
00221 
00222     GridState::Ptr start_state = graph_->getState(sx, sy);
00223     GridState::Ptr goal_state = graph_->getState(gx, gy);
00224 
00225     if(start_state->getOccupancy() != 0) {
00226       ROS_ERROR("start state is occupied");
00227       as_.setPreempted();
00228       return;
00229     }
00230     if(goal_state->getOccupancy() != 0) {
00231       ROS_ERROR("goal state is occupied");
00232       as_.setPreempted();
00233       return;
00234     }
00235 
00236     graph_->setStartState(start_state);
00237     graph_->setGoalState(goal_state);
00238 
00239     Solver solver(graph_);
00240     //solver.setHeuristic(boost::bind(&gridPerceptionHeuristicDistance, _1, _2));
00241     solver.setHeuristic(boost::bind(&GridPathPlanner::heuristicDistance, this, _1, _2));
00242 
00243     Solver::Path path = solver.solve();
00244     if(path.size() < 1) {
00245       ROS_ERROR("Failed to plan path");
00246       ROS_INFO("pub marker"); //debug
00247       publishMarker();        //debug
00248       as_.setPreempted();
00249       return;
00250     }
00251     ROS_INFO("solved path_size: %d", path.size());
00252     std::vector<Eigen::Vector3f > points;
00253     for(int i = 0; i < path.size(); i++) {
00254       //Solver::StatePtr st = path[i]->getState();
00255       int ix = path[i]->getState()->indexX();
00256       int iy = path[i]->getState()->indexY();
00257       Eigen::Vector3f p;
00258       gridToPoint(ix, iy, p);
00259       points.push_back(p);
00260       ROS_INFO("path %d (%f %f) [%d - %d]", i, ix, iy, p[0], p[1]);
00261     }
00262     {
00263       jsk_recognition_utils::PolyLine pl(points);
00264       visualization_msgs::MarkerArray ma;
00265       visualization_msgs::Marker m;
00266       pl.toMarker(m);
00267       m.header.frame_id = "map";
00268       m.id = 101;
00269       m.scale.x = 0.05;
00270       ma.markers.push_back(m);
00271       pub_marker_.publish(ma);
00272     }
00273     ROS_INFO("pub marker");
00274     publishMarker();
00275 
00276     {
00277       jsk_footstep_msgs::PlanFootstepsResult result_;
00278       result_.result.header = goal->goal_footstep.header;
00279 
00280       std::vector<Eigen::Vector3f > points;
00281       for(int i = 0; i < path.size(); i++) {
00282         int ix = path[i]->getState()->indexX();
00283         int iy = path[i]->getState()->indexY();
00284         Eigen::Vector3f p;
00285         gridToPoint(ix, iy, p);
00286         jsk_footstep_msgs::Footstep fs;
00287         fs.leg = jsk_footstep_msgs::Footstep::LLEG;
00288         fs.dimensions.x = map_resolution_;
00289         fs.dimensions.y = map_resolution_;
00290         fs.dimensions.z = 0.01;
00291         fs.pose.orientation.x = 0;
00292         fs.pose.orientation.y = 0;
00293         fs.pose.orientation.z = 0;
00294         fs.pose.orientation.w = 1;
00295         fs.pose.position.x = p[0];
00296         fs.pose.position.y = p[1];
00297         fs.pose.position.z = p[2];
00298         result_.result.footsteps.push_back(fs);
00299       }
00300       as_.setSucceeded(result_);
00301     }
00302 
00303 #if 0
00304     pcl::PointCloud<pcl::PointXYZRGB> close_list_cloud, open_list_cloud;
00305     //solver.openListToPointCloud(open_list_cloud);
00306     //solver.closeListToPointCloud(close_list_cloud);
00307     publishPointCloud(close_list_cloud, pub_close_list_, goal->goal_footstep.header);
00308     publishPointCloud(open_list_cloud,  pub_open_list_,  goal->goal_footstep.header);
00309 #endif
00310 
00311 #if 0
00312     publishText(pub_text_,
00313                 (boost::format("Took %f sec\nPerception took %f sec\nPlanning took %f sec\n%lu path\nopen list: %lu\nclose list:%lu")
00314                  % planning_duration 
00315                  % graph_->getPerceptionDuration().toSec()
00316                  % (planning_duration - graph_->getPerceptionDuration().toSec())
00317                  % path.size()
00318                  % open_list_cloud.points.size()
00319                  % close_list_cloud.points.size()).str(),
00320                 OK);
00321     ROS_INFO_STREAM("use_obstacle_points: " << graph_->useObstaclePoints());
00322 #endif
00323   }
00324 
00325   void GridPathPlanner::publishPointCloud(
00326     const pcl::PointCloud<pcl::PointXYZRGB>& cloud,
00327     ros::Publisher& pub,
00328     const std_msgs::Header& header)
00329   {
00330     sensor_msgs::PointCloud2 ros_cloud;
00331     pcl::toROSMsg(cloud, ros_cloud);
00332     ros_cloud.header = header;
00333     pub.publish(ros_cloud);
00334   }
00335 
00336   bool GridPathPlanner::updateCost(GridState::Ptr ptr)
00337   {
00338     // determine occupancy
00339     if(use_obstacle_points_)
00340     {
00341       Eigen::Vector3f p;
00342       gridToPoint(ptr->indexX(), ptr->indexY(), p);
00343 
00344       double pos0 = collision_circle_min_height_ + collision_circle_radius_;
00345       //double pos1 = collision_circle_max_height_ - collision_circle_radius_;
00346       int size = 0;
00347       std::vector<double> pos;
00348       pos.push_back(pos0);
00349 
00350       for(std::vector<double>::iterator it = pos.begin();
00351           it != pos.end(); it++) {
00352         std::vector<int> near_indices;
00353         std::vector<float> distances;
00354         pcl::PointXYZ center;
00355         p[2] = *it;
00356         center.getVector3fMap() = p;
00357 
00358         obstacle_tree_->radiusSearch(center, collision_circle_radius_, near_indices, distances);
00359 
00360         //std::cerr << "oz: " << near_indices.size() << std::endl;
00361         size += near_indices.size();
00362       }
00363       if(size > 1) { // TODO: occupancy_threshold
00364         ptr->setOccupancy(1);
00365       } else {
00366         ptr->setOccupancy(0);
00367       }
00368     }
00369 
00370     // determine cost
00371     if(use_plane_points_)
00372     {
00373       Eigen::Vector3f p;
00374       gridToPoint(ptr->indexX(), ptr->indexY(), p);
00375 
00376       std::vector<int> plane_indices;
00377       std::vector<float> distances;
00378       pcl::PointNormal center;
00379       center.getVector3fMap() = p;
00380       plane_tree_->radiusSearch(center, map_resolution_ * 1.4, plane_indices, distances);
00381       //std::cerr << "ps: " << plane_indices.size() << std::endl;
00382       // TODO: point num / point average / point deviation
00383       double cost = 0.0;
00384       if (plane_indices.size() < 50) { // TODO: plane threshold
00385         ptr->setOccupancy(2 + ptr->getOccupancy());
00386       }
00387       ptr->setCost(cost);
00388     }
00389 
00390     return false;
00391   }
00392 
00393   void GridPathPlanner::publishMarker()
00394   {
00395     visualization_msgs::MarkerArray ma;
00396 
00397     int sx = gridmap_->sizeX();
00398     int sy = gridmap_->sizeY();
00399 
00400     { // state plane
00401       visualization_msgs::Marker m;
00402       m.header.frame_id = "map";
00403       m.type = visualization_msgs::Marker::CUBE_LIST;
00404       m.id = 100;
00405       m.scale.x = map_resolution_;
00406       m.scale.y = map_resolution_;
00407       m.scale.z = 0.01;
00408       m.color.r = 0.0;
00409       m.color.g = 0.0;
00410       m.color.b = 1.0;
00411       m.color.a = 1.0;
00412 
00413       for(int y = 0; y < sy; y++) {
00414         for(int x = 0; x < sx; x++) {
00415           GridState::Ptr st = graph_->getState(x, y);
00416           Eigen::Vector3f p;
00417           gridToPoint(x, y, p);
00418           p[2] = -0.005;
00419           geometry_msgs::Point gp;
00420           gp.x = p[0];
00421           gp.y = p[1];
00422           gp.z = p[2];
00423           m.points.push_back(gp);
00424 
00425           std_msgs::ColorRGBA col;
00426           if(st->getOccupancy() == 1) {
00427             col.r = 0.0;
00428             col.g = 0.15;
00429             col.b = 0.0;
00430             col.a = 1.0;
00431           } else if(st->getOccupancy() == 2) {
00432             col.r = 0.0;
00433             col.g = 0.0;
00434             col.b = 0.0;
00435             col.a = 1.0;
00436           } else if(st->getOccupancy() == 3) {
00437             col.r = 0.15;
00438             col.g = 0.0;
00439             col.b = 0.0;
00440             col.a = 1.0;
00441           } else {
00442             col.r = 0.0;
00443             col.g = 0.0;
00444             col.b = 1.0;
00445             col.a = 1.0;
00446           }
00447 
00448           m.colors.push_back(col);
00449         }
00450       }
00451       ma.markers.push_back(m);
00452     }
00453 
00454     pub_marker_.publish(ma);
00455   }
00456 }


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