void FootstepPlanner::profile(FootstepAStarSolver<FootstepGraph>& solver, FootstepGraph::Ptr graph)
 {
   JSK_ROS_INFO("open list: %lu", solver.getOpenList().size());
   JSK_ROS_INFO("close list: %lu", solver.getCloseList().size());
   publishText(pub_text_,
               (boost::format("open_list: %lu\nclose list:%lu")
                % (solver.getOpenList().size()) % (solver.getCloseList().size())).str(),
               OK);
   if (rich_profiling_) {
     pcl::PointCloud<pcl::PointNormal> close_list_cloud, open_list_cloud;
     solver.openListToPointCloud(open_list_cloud);
     solver.closeListToPointCloud(close_list_cloud);
     publishPointCloud(close_list_cloud, pub_close_list_, latest_header_);
     publishPointCloud(open_list_cloud, pub_open_list_, latest_header_);
   }
 }
void profile(FootstepAStarSolver<FootstepGraph>& solver, FootstepGraph::Ptr graph)
{
  ROS_INFO("open list: %lu", solver.getOpenList().size());
  ROS_INFO("close list: %lu", solver.getCloseList().size());
}