bench_footstep_planner.cpp
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 #include <jsk_footstep_msgs/FootstepArray.h>
00036 #include "jsk_footstep_planner/footstep_graph.h"
00037 #include "jsk_footstep_planner/astar_solver.h"
00038 #include "jsk_footstep_planner/footstep_astar_solver.h"
00039 #include <time.h>
00040 #include <boost/random.hpp>
00041 #include <fstream>
00042 #include <boost/program_options.hpp>
00043 #include <boost/algorithm/string.hpp>
00044 #include "jsk_footstep_planner/pointcloud_model_generator.h"
00045 
00046 using namespace jsk_footstep_planner;
00047 
00048 const Eigen::Vector3f footstep_size(0.2, 0.1, 0.000001);
00049 
00050 #define OPTION_DEFAULT_VALUE(TYPE, VALUE) boost::program_options::value<TYPE>()->default_value(VALUE)
00051 #define OPTION_TYPE(TYPE) boost::program_options::value<TYPE>()
00052 
00053 void setupGraph(FootstepGraph::Ptr graph, Eigen::Vector3f res)
00054 {
00055   std::vector<Eigen::Affine3f> successors;
00056   successors.push_back(affineFromXYYaw(0, -0.2, 0));
00057   successors.push_back(affineFromXYYaw(0, -0.3, 0));
00058   successors.push_back(affineFromXYYaw(0, -0.15, 0));
00059   successors.push_back(affineFromXYYaw(0.2, -0.2, 0));
00060   successors.push_back(affineFromXYYaw(0.25, -0.2, 0));
00061   successors.push_back(affineFromXYYaw(0.1, -0.2, 0));
00062   successors.push_back(affineFromXYYaw(-0.1, -0.2, 0));
00063   successors.push_back(affineFromXYYaw(0, -0.2, 0.17));
00064   successors.push_back(affineFromXYYaw(0, -0.3, 0.17));
00065   successors.push_back(affineFromXYYaw(0.2, -0.2, 0.17));
00066   successors.push_back(affineFromXYYaw(0.25, -0.2, 0.17));
00067   successors.push_back(affineFromXYYaw(0.1, -0.2, 0.17));
00068   successors.push_back(affineFromXYYaw(0, -0.2, -0.17));
00069   successors.push_back(affineFromXYYaw(0, -0.3, -0.17));
00070   successors.push_back(affineFromXYYaw(0.2, -0.2, -0.17));
00071   successors.push_back(affineFromXYYaw(0.25, -0.2, -0.17));
00072   successors.push_back(affineFromXYYaw(0.1, -0.2, -0.17));
00073   FootstepState::Ptr start(new FootstepState(jsk_footstep_msgs::Footstep::LEFT,
00074                                              Eigen::Affine3f::Identity(),
00075                                              footstep_size,
00076                                              res));
00077   graph->setStartState(start);
00078   graph->setBasicSuccessors(successors);
00079 }
00080 
00081 inline bool planBench(double x, double y, double yaw,
00082                       FootstepGraph::Ptr graph,
00083                       Eigen::Vector3f footstep_size,
00084                       const std::string heuristic,
00085                       const double first_rotation_weight,
00086                       const double second_rotation_weight,
00087                       Eigen::Vector3f res)
00088 {
00089   Eigen::Affine3f goal_center = affineFromXYYaw(x, y, yaw);
00090   FootstepState::Ptr left_goal(new FootstepState(jsk_footstep_msgs::Footstep::LEFT,
00091                                                  goal_center * Eigen::Translation3f(0, 0.1, 0),
00092                                                  footstep_size,
00093                                                  res));
00094   FootstepState::Ptr right_goal(new FootstepState(jsk_footstep_msgs::Footstep::RIGHT,
00095                                                   goal_center * Eigen::Translation3f(0, -0.1, 0),
00096                                                   footstep_size,
00097                                                   res));
00098   graph->setGoalState(left_goal, right_goal);
00099   // Project goal and start
00100   if (graph->usePointCloudModel()) {
00101     if (!graph->projectGoal()) {
00102       ROS_FATAL("Failed to project goal");
00103       return false;
00104     }
00105   }
00106   //AStarSolver<FootstepGraph> solver(graph);
00107   FootstepAStarSolver<FootstepGraph> solver(graph, 100, 100, 100);
00108   if (heuristic == "step_cost") {
00109     solver.setHeuristic(boost::bind(&footstepHeuristicStepCost, _1, _2, first_rotation_weight, second_rotation_weight));
00110   }
00111   else if (heuristic == "zero") {
00112     solver.setHeuristic(&footstepHeuristicZero);
00113   }
00114   else if (heuristic == "straight") {
00115     solver.setHeuristic(&footstepHeuristicStraight);
00116   }
00117   else if (heuristic == "straight_rotation") {
00118     solver.setHeuristic(&footstepHeuristicStraightRotation);
00119   }
00120   std::vector<SolverNode<FootstepState, FootstepGraph>::Ptr> path = solver.solve(ros::WallDuration(10.0));
00121   return true;
00122 }
00123 
00124 inline void progressBar(int x, int n, int w)
00125 {
00126     // Calculuate the ratio of complete-to-incomplete.
00127     float ratio = x/(float)n;
00128     int   c     = ratio * w;
00129  
00130     // Show the percentage complete.
00131     printf("%d/%d (%d%) [", x, n, (int)(ratio*100) );
00132  
00133     // Show the load bar.
00134     for (int x=0; x<c; x++)
00135        printf("=");
00136  
00137     for (int x=c; x<w; x++)
00138        printf(" ");
00139  
00140     // ANSI Control codes to go back to the
00141     // previous line and clear it.
00142     printf("]\n\033[F\033[J");
00143 }
00144 
00145 
00146 int main(int argc, char** argv)
00147 {
00148   ros::init(argc, argv, "bench_footstep_planner", ros::init_options::AnonymousName);
00149   ros::NodeHandle nh("~");
00150   boost::program_options::options_description opt("Options");
00151   opt.add_options()
00152     ("min_x",OPTION_DEFAULT_VALUE(double, -0.3), "minimum x")
00153     ("max_x", OPTION_DEFAULT_VALUE(double, 3.0), "maximum x")
00154     ("dx", OPTION_DEFAULT_VALUE(double, 0.2), "x resolution")
00155     ("min_y", OPTION_DEFAULT_VALUE(double, -3.0), "minimum y")
00156     ("max_y", OPTION_DEFAULT_VALUE(double, 3.0), "maximum y")
00157     ("dy", OPTION_DEFAULT_VALUE(double, 0.2), "y resolution")
00158     ("resolution_x", OPTION_DEFAULT_VALUE(double, 0.05), "x resolution of local grid")
00159     ("resolution_y", OPTION_DEFAULT_VALUE(double, 0.05), "y resolution of local grid")
00160     ("resolution_theta", OPTION_DEFAULT_VALUE(double, 0.08), "theta resolution of local grid")
00161     ("n_theta", OPTION_DEFAULT_VALUE(int, 8), "theta resolution.")
00162     ("heuristic", OPTION_DEFAULT_VALUE(std::string, std::string("step_cost")),
00163      "heuristic function (zero, straight, straight_rotation, step_cost)")
00164     ("first_rotation_weight", OPTION_DEFAULT_VALUE(double, 1.0),
00165      "first rotation weight of step_cost heuristic")
00166     ("second_rotation_weight", OPTION_DEFAULT_VALUE(double, 1.0),
00167      "second rotation weight of step_cost heuristic")
00168     ("model", OPTION_DEFAULT_VALUE(std::string, std::string("none")),
00169      "pointcloud model (none, flat, stairs, hilld, gaussian)")
00170     ("enable_lazy_perception", "Enable Lazy Perception")
00171     ("enable_local_movement", "Enable Local Movement")
00172     ("test_count", OPTION_DEFAULT_VALUE(int, 10), "the number of test times to take average")
00173     ("output,o", OPTION_DEFAULT_VALUE(std::string, std::string("output.csv")), "output file")
00174     ("verbose,v", "verbose")
00175     ("help,h", "Show help");
00176   
00177   boost::program_options::variables_map vm;
00178   boost::program_options::store(boost::program_options::parse_command_line(argc, argv, opt), vm);
00179   boost::program_options::notify(vm);
00180 
00181   if (vm.count("help")) {
00182     std::cout << opt << std::endl;
00183     return 0;
00184   }
00185 
00186   const double min_x = vm["min_x"].as<double>();
00187   const double max_x = vm["max_x"].as<double>();
00188   const double dx = vm["dx"].as<double>();
00189   const double min_y = vm["min_y"].as<double>();
00190   const double max_y = vm["max_y"].as<double>();
00191   const double dy = vm["dy"].as<double>();
00192   const double resolution_x = vm["resolution_x"].as<double>();
00193   const double resolution_y = vm["resolution_y"].as<double>();
00194   const double resolution_theta = vm["resolution_theta"].as<double>();
00195   const int n_theta = vm["n_theta"].as<int>();
00196   const std::string heuristic = vm["heuristic"].as<std::string>();
00197   const double first_rotation_weight = vm["first_rotation_weight"].as<double>();
00198   const double second_rotation_weight = vm["second_rotation_weight"].as<double>();
00199   const std::string model = vm["model"].as<std::string>();
00200   const bool enable_perception = vm["model"].as<std::string>() != "none";
00201   const bool enable_lazy_perception = vm.count("enable_lazy_perception") > 0;
00202   const bool enable_local_movement = vm.count("enable_local_movement") > 0;
00203   const bool verbose = vm.count("verbose") > 0;
00204   const int test_count = vm["test_count"].as<int>();
00205   const std::string output_filename = vm["output"].as<std::string>();
00206 
00207   // Print parameters
00208   std::cout << "parameters" << std::endl;
00209   std::cout << "  min_x: " << min_x << std::endl;
00210   std::cout << "  max_x: " << max_x << std::endl;
00211   std::cout << "  dx: " << dx << std::endl;
00212   std::cout << "  min_y: " << min_y << std::endl;
00213   std::cout << "  max_y: " << max_y << std::endl;
00214   std::cout << "  dy: " << dy << std::endl;
00215   std::cout << "  resolution_x: " << resolution_x << std::endl;
00216   std::cout << "  resolution_y: " << resolution_y << std::endl;
00217   std::cout << "  resolution_theta: " << resolution_theta << std::endl;
00218   std::cout << "  n_theta: " << n_theta << std::endl;
00219   std::cout << "  heuristic: " << heuristic << std::endl;
00220   std::cout << "  first_rotation_weight: " << first_rotation_weight << std::endl;
00221   std::cout << "  second_rotation_weight: " << second_rotation_weight << std::endl;
00222   std::cout << "  model: " << model << std::endl;
00223   std::cout << "  enable_lazy_perception: " << enable_lazy_perception << std::endl;
00224   std::cout << "  enable_local_movement: " << enable_local_movement << std::endl;
00225   std::cout << "  test_count: " << test_count << std::endl;
00226   std::cout << "  output: " << output_filename << std::endl;
00227   std::cout << "  verbose: " << verbose << std::endl;
00228   
00229   PointCloudModelGenerator generator;
00230   pcl::PointCloud<pcl::PointNormal>::Ptr cloud (new pcl::PointCloud<pcl::PointNormal>);
00231   if (model != "none") {
00232     generator.generate(model, *cloud);
00233   }
00234   Eigen::Vector3f res(resolution_x, resolution_y, resolution_theta);
00235   FootstepGraph::Ptr graph(new FootstepGraph(res, enable_perception, enable_lazy_perception, enable_local_movement));
00236   if (enable_perception) {
00237     graph->setPointCloudModel(cloud);
00238     
00239   }
00240   setupGraph(graph, res);
00241   if (enable_perception) {
00242     if (!graph->projectStart()) {
00243       ROS_FATAL("Failed to project start state");
00244       return 1;
00245     }
00246   }
00247   std::cout << graph->infoString() << std::endl;
00248   std::ofstream ofs(output_filename.c_str());
00249   int test_num = n_theta * ceil((max_x - min_x) / dx + 1) * ceil((max_y - min_y) / dy + 1);
00250   int count = 0;
00251   // First line, indices
00252   ofs << "x,y,theta,one_time,"
00253       << "min_x,max_x,dx,min_y,max_y,dy,"
00254       << "resolution_x,resolution_y,resolution_theta,n_theta,"
00255       << "heuristic,first_rotation_weight,second_rotation_weight,"
00256       << "model,"
00257       << "enable_lazy_perception,enable_local_movement,"
00258       << "test_count" << std::endl;
00259   
00260   for (size_t ti = 0; ti < n_theta; ti++) {
00261     double theta = 2.0 * M_PI / n_theta * ti ;
00262     for (double x = min_x; x <= max_x; x += dx) {
00263       for (double y = min_y; y <= max_y; y += dy) {
00264         ++count;
00265         progressBar(count, test_num, 80);
00266         ros::WallTime start = ros::WallTime::now();
00267         // Run plan function for test_count
00268         bool success = false;
00269         for (size_t i = 0; i < test_count; i++) {
00270           success = planBench(x, y, theta, graph, footstep_size, heuristic, second_rotation_weight, second_rotation_weight, res);
00271         }
00272         ros::WallTime end = ros::WallTime::now();
00273         if (!ros::ok()) {
00274           return 0;
00275         }
00276         if (verbose) {
00277           std::cout << "Planning took " << (end-start).toSec()/test_count << " sec" << std::endl;
00278         }
00279         if (success) {
00280           double time_to_solve = (end - start).toSec() / test_count;
00281           ofs << x << "," << y << "," << theta << "," << time_to_solve
00282               << "," << min_x << "," << max_x << "," << dx << "," << min_y << "," << max_y << "," << dy
00283               << "," << resolution_x << "," << resolution_y << "," << resolution_theta << "," << n_theta
00284               << "," << heuristic << "," << first_rotation_weight << "," << second_rotation_weight
00285               << "," << model
00286               << "," << enable_lazy_perception << "," << enable_local_movement
00287               << "," << test_count << std::endl;
00288         }
00289         else {
00290           ROS_FATAL("Failed to plan");
00291         }
00292         
00293       }
00294       //std::cout << std::endl;
00295     }
00296   }
00297   std::cout << "Plot result by scripts/plot_bench.py" << std::endl;
00298   return 0;
00299 }
00300 


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