bench_footstep_planner.cpp
Go to the documentation of this file.
1 // -*- mode: c++ -*-
2 /*********************************************************************
3  * Software License Agreement (BSD License)
4  *
5  * Copyright (c) 2015, JSK Lab
6  * All rights reserved.
7  *
8  * Redistribution and use in source and binary forms, with or without
9  * modification, are permitted provided that the following conditions
10  * are met:
11  *
12  * * Redistributions of source code must retain the above copyright
13  * notice, this list of conditions and the following disclaimer.
14  * * Redistributions in binary form must reproduce the above
15  * copyright notice, this list of conditions and the following
16  * disclaimer in the documentation and/o2r other materials provided
17  * with the distribution.
18  * * Neither the name of the JSK Lab nor the names of its
19  * contributors may be used to endorse or promote products derived
20  * from this software without specific prior written permission.
21  *
22  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
23  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
24  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
25  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
26  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
27  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
28  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
29  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
30  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
31  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
32  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
33  * POSSIBILITY OF SUCH DAMAGE.
34  *********************************************************************/
35 #include <jsk_footstep_msgs/FootstepArray.h>
39 #include <time.h>
40 #include <boost/random.hpp>
41 #include <fstream>
42 #include <boost/program_options.hpp>
43 #include <boost/algorithm/string.hpp>
45 
46 using namespace jsk_footstep_planner;
47 
48 const Eigen::Vector3f footstep_size(0.2, 0.1, 0.000001);
49 
50 #define OPTION_DEFAULT_VALUE(TYPE, VALUE) boost::program_options::value<TYPE>()->default_value(VALUE)
51 #define OPTION_TYPE(TYPE) boost::program_options::value<TYPE>()
52 
53 void setupGraph(FootstepGraph::Ptr graph, Eigen::Vector3f res)
54 {
55  std::vector<Eigen::Affine3f> successors;
56  successors.push_back(affineFromXYYaw(0, -0.2, 0));
57  successors.push_back(affineFromXYYaw(0, -0.3, 0));
58  successors.push_back(affineFromXYYaw(0, -0.15, 0));
59  successors.push_back(affineFromXYYaw(0.2, -0.2, 0));
60  successors.push_back(affineFromXYYaw(0.25, -0.2, 0));
61  successors.push_back(affineFromXYYaw(0.1, -0.2, 0));
62  successors.push_back(affineFromXYYaw(-0.1, -0.2, 0));
63  successors.push_back(affineFromXYYaw(0, -0.2, 0.17));
64  successors.push_back(affineFromXYYaw(0, -0.3, 0.17));
65  successors.push_back(affineFromXYYaw(0.2, -0.2, 0.17));
66  successors.push_back(affineFromXYYaw(0.25, -0.2, 0.17));
67  successors.push_back(affineFromXYYaw(0.1, -0.2, 0.17));
68  successors.push_back(affineFromXYYaw(0, -0.2, -0.17));
69  successors.push_back(affineFromXYYaw(0, -0.3, -0.17));
70  successors.push_back(affineFromXYYaw(0.2, -0.2, -0.17));
71  successors.push_back(affineFromXYYaw(0.25, -0.2, -0.17));
72  successors.push_back(affineFromXYYaw(0.1, -0.2, -0.17));
73  FootstepState::Ptr start(new FootstepState(jsk_footstep_msgs::Footstep::LEFT,
74  Eigen::Affine3f::Identity(),
76  res));
77  graph->setStartState(start);
78  graph->setBasicSuccessors(successors);
79 }
80 
81 inline bool planBench(double x, double y, double yaw,
83  Eigen::Vector3f footstep_size,
84  const std::string heuristic,
85  const double first_rotation_weight,
86  const double second_rotation_weight,
87  Eigen::Vector3f res)
88 {
89  Eigen::Affine3f goal_center = affineFromXYYaw(x, y, yaw);
90  FootstepState::Ptr left_goal(new FootstepState(jsk_footstep_msgs::Footstep::LEFT,
91  goal_center * Eigen::Translation3f(0, 0.1, 0),
92  footstep_size,
93  res));
94  FootstepState::Ptr right_goal(new FootstepState(jsk_footstep_msgs::Footstep::RIGHT,
95  goal_center * Eigen::Translation3f(0, -0.1, 0),
96  footstep_size,
97  res));
98  graph->setGoalState(left_goal, right_goal);
99  // Project goal and start
100  if (graph->usePointCloudModel()) {
101  if (!graph->projectGoal()) {
102  ROS_FATAL("Failed to project goal");
103  return false;
104  }
105  }
106  //AStarSolver<FootstepGraph> solver(graph);
107  FootstepAStarSolver<FootstepGraph> solver(graph, 100, 100, 100);
108  if (heuristic == "step_cost") {
109  solver.setHeuristic(boost::bind(&footstepHeuristicStepCost, _1, _2, first_rotation_weight, second_rotation_weight));
110  }
111  else if (heuristic == "zero") {
113  }
114  else if (heuristic == "straight") {
116  }
117  else if (heuristic == "straight_rotation") {
119  }
120  std::vector<SolverNode<FootstepState, FootstepGraph>::Ptr> path = solver.solve(ros::WallDuration(10.0));
121  return true;
122 }
123 
124 inline void progressBar(int x, int n, int w)
125 {
126  // Calculuate the ratio of complete-to-incomplete.
127  float ratio = x/(float)n;
128  int c = ratio * w;
129 
130  // Show the percentage complete.
131  printf("%d/%d (%d%) [", x, n, (int)(ratio*100) );
132 
133  // Show the load bar.
134  for (int x=0; x<c; x++)
135  printf("=");
136 
137  for (int x=c; x<w; x++)
138  printf(" ");
139 
140  // ANSI Control codes to go back to the
141  // previous line and clear it.
142  printf("]\n\033[F\033[J");
143 }
144 
145 
146 int main(int argc, char** argv)
147 {
148  ros::init(argc, argv, "bench_footstep_planner", ros::init_options::AnonymousName);
149  ros::NodeHandle nh("~");
150  boost::program_options::options_description opt("Options");
151  opt.add_options()
152  ("min_x",OPTION_DEFAULT_VALUE(double, -0.3), "minimum x")
153  ("max_x", OPTION_DEFAULT_VALUE(double, 3.0), "maximum x")
154  ("dx", OPTION_DEFAULT_VALUE(double, 0.2), "x resolution")
155  ("min_y", OPTION_DEFAULT_VALUE(double, -3.0), "minimum y")
156  ("max_y", OPTION_DEFAULT_VALUE(double, 3.0), "maximum y")
157  ("dy", OPTION_DEFAULT_VALUE(double, 0.2), "y resolution")
158  ("resolution_x", OPTION_DEFAULT_VALUE(double, 0.05), "x resolution of local grid")
159  ("resolution_y", OPTION_DEFAULT_VALUE(double, 0.05), "y resolution of local grid")
160  ("resolution_theta", OPTION_DEFAULT_VALUE(double, 0.08), "theta resolution of local grid")
161  ("n_theta", OPTION_DEFAULT_VALUE(int, 8), "theta resolution.")
162  ("heuristic", OPTION_DEFAULT_VALUE(std::string, std::string("step_cost")),
163  "heuristic function (zero, straight, straight_rotation, step_cost)")
164  ("first_rotation_weight", OPTION_DEFAULT_VALUE(double, 1.0),
165  "first rotation weight of step_cost heuristic")
166  ("second_rotation_weight", OPTION_DEFAULT_VALUE(double, 1.0),
167  "second rotation weight of step_cost heuristic")
168  ("model", OPTION_DEFAULT_VALUE(std::string, std::string("none")),
169  "pointcloud model (none, flat, stairs, hilld, gaussian)")
170  ("enable_lazy_perception", "Enable Lazy Perception")
171  ("enable_local_movement", "Enable Local Movement")
172  ("test_count", OPTION_DEFAULT_VALUE(int, 10), "the number of test times to take average")
173  ("output,o", OPTION_DEFAULT_VALUE(std::string, std::string("output.csv")), "output file")
174  ("verbose,v", "verbose")
175  ("help,h", "Show help");
176 
177  boost::program_options::variables_map vm;
178  boost::program_options::store(boost::program_options::parse_command_line(argc, argv, opt), vm);
179  boost::program_options::notify(vm);
180 
181  if (vm.count("help")) {
182  std::cout << opt << std::endl;
183  return 0;
184  }
185 
186  const double min_x = vm["min_x"].as<double>();
187  const double max_x = vm["max_x"].as<double>();
188  const double dx = vm["dx"].as<double>();
189  const double min_y = vm["min_y"].as<double>();
190  const double max_y = vm["max_y"].as<double>();
191  const double dy = vm["dy"].as<double>();
192  const double resolution_x = vm["resolution_x"].as<double>();
193  const double resolution_y = vm["resolution_y"].as<double>();
194  const double resolution_theta = vm["resolution_theta"].as<double>();
195  const int n_theta = vm["n_theta"].as<int>();
196  const std::string heuristic = vm["heuristic"].as<std::string>();
197  const double first_rotation_weight = vm["first_rotation_weight"].as<double>();
198  const double second_rotation_weight = vm["second_rotation_weight"].as<double>();
199  const std::string model = vm["model"].as<std::string>();
200  const bool enable_perception = vm["model"].as<std::string>() != "none";
201  const bool enable_lazy_perception = vm.count("enable_lazy_perception") > 0;
202  const bool enable_local_movement = vm.count("enable_local_movement") > 0;
203  const bool verbose = vm.count("verbose") > 0;
204  const int test_count = vm["test_count"].as<int>();
205  const std::string output_filename = vm["output"].as<std::string>();
206 
207  // Print parameters
208  std::cout << "parameters" << std::endl;
209  std::cout << " min_x: " << min_x << std::endl;
210  std::cout << " max_x: " << max_x << std::endl;
211  std::cout << " dx: " << dx << std::endl;
212  std::cout << " min_y: " << min_y << std::endl;
213  std::cout << " max_y: " << max_y << std::endl;
214  std::cout << " dy: " << dy << std::endl;
215  std::cout << " resolution_x: " << resolution_x << std::endl;
216  std::cout << " resolution_y: " << resolution_y << std::endl;
217  std::cout << " resolution_theta: " << resolution_theta << std::endl;
218  std::cout << " n_theta: " << n_theta << std::endl;
219  std::cout << " heuristic: " << heuristic << std::endl;
220  std::cout << " first_rotation_weight: " << first_rotation_weight << std::endl;
221  std::cout << " second_rotation_weight: " << second_rotation_weight << std::endl;
222  std::cout << " model: " << model << std::endl;
223  std::cout << " enable_lazy_perception: " << enable_lazy_perception << std::endl;
224  std::cout << " enable_local_movement: " << enable_local_movement << std::endl;
225  std::cout << " test_count: " << test_count << std::endl;
226  std::cout << " output: " << output_filename << std::endl;
227  std::cout << " verbose: " << verbose << std::endl;
228 
229  PointCloudModelGenerator generator;
230  pcl::PointCloud<pcl::PointNormal>::Ptr cloud (new pcl::PointCloud<pcl::PointNormal>);
231  if (model != "none") {
232  generator.generate(model, *cloud);
233  }
234  Eigen::Vector3f res(resolution_x, resolution_y, resolution_theta);
235  FootstepGraph::Ptr graph(new FootstepGraph(res, enable_perception, enable_lazy_perception, enable_local_movement));
236  if (enable_perception) {
237  graph->setPointCloudModel(cloud);
238 
239  }
240  setupGraph(graph, res);
241  if (enable_perception) {
242  if (!graph->projectStart()) {
243  ROS_FATAL("Failed to project start state");
244  return 1;
245  }
246  }
247  std::cout << graph->infoString() << std::endl;
248  std::ofstream ofs(output_filename.c_str());
249  int test_num = n_theta * ceil((max_x - min_x) / dx + 1) * ceil((max_y - min_y) / dy + 1);
250  int count = 0;
251  // First line, indices
252  ofs << "x,y,theta,one_time,"
253  << "min_x,max_x,dx,min_y,max_y,dy,"
254  << "resolution_x,resolution_y,resolution_theta,n_theta,"
255  << "heuristic,first_rotation_weight,second_rotation_weight,"
256  << "model,"
257  << "enable_lazy_perception,enable_local_movement,"
258  << "test_count" << std::endl;
259 
260  for (size_t ti = 0; ti < n_theta; ti++) {
261  double theta = 2.0 * M_PI / n_theta * ti ;
262  for (double x = min_x; x <= max_x; x += dx) {
263  for (double y = min_y; y <= max_y; y += dy) {
264  ++count;
265  progressBar(count, test_num, 80);
267  // Run plan function for test_count
268  bool success = false;
269  for (size_t i = 0; i < test_count; i++) {
270  success = planBench(x, y, theta, graph, footstep_size, heuristic, second_rotation_weight, second_rotation_weight, res);
271  }
273  if (!ros::ok()) {
274  return 0;
275  }
276  if (verbose) {
277  std::cout << "Planning took " << (end-start).toSec()/test_count << " sec" << std::endl;
278  }
279  if (success) {
280  double time_to_solve = (end - start).toSec() / test_count;
281  ofs << x << "," << y << "," << theta << "," << time_to_solve
282  << "," << min_x << "," << max_x << "," << dx << "," << min_y << "," << max_y << "," << dy
283  << "," << resolution_x << "," << resolution_y << "," << resolution_theta << "," << n_theta
284  << "," << heuristic << "," << first_rotation_weight << "," << second_rotation_weight
285  << "," << model
286  << "," << enable_lazy_perception << "," << enable_local_movement
287  << "," << test_count << std::endl;
288  }
289  else {
290  ROS_FATAL("Failed to plan");
291  }
292 
293  }
294  //std::cout << std::endl;
295  }
296  }
297  std::cout << "Plot result by scripts/plot_bench.py" << std::endl;
298  return 0;
299 }
300 
double footstepHeuristicStepCost(SolverNode< FootstepState, FootstepGraph >::Ptr node, FootstepGraph::Ptr graph, double first_rotation_weight, double second_rotation_weight)
#define ROS_FATAL(...)
path
end
bool planBench(double x, double y, double yaw, FootstepGraph::Ptr graph, Eigen::Vector3f footstep_size, const std::string heuristic, const double first_rotation_weight, const double second_rotation_weight, Eigen::Vector3f res)
Eigen::Affine3f affineFromXYYaw(double x, double y, double yaw)
Definition: util.h:42
double footstepHeuristicStraightRotation(SolverNode< FootstepState, FootstepGraph >::Ptr node, FootstepGraph::Ptr graph)
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
opt
void setupGraph(FootstepGraph::Ptr graph, Eigen::Vector3f res)
double y
std::string model
double footstepHeuristicStraight(SolverNode< FootstepState, FootstepGraph >::Ptr node, FootstepGraph::Ptr graph)
float
c
bool verbose
#define M_PI
void progressBar(int x, int n, int w)
virtual std::vector< typename SolverNode< State, GraphT >::Ptr > solve(const ros::WallDuration &timeout=ros::WallDuration(1000000000.0))
pcl::PointCloud< pcl::PointNormal >::Ptr cloud
virtual void setHeuristic(HeuristicFunction h)
Definition: astar_solver.h:71
ROSCPP_DECL bool ok()
FootstepGraph::Ptr graph
start
virtual void generate(const std::string &model_name, pcl::PointCloud< PointT > &output, double hole_rate=0.0)
res
#define OPTION_DEFAULT_VALUE(TYPE, VALUE)
double footstepHeuristicZero(SolverNode< FootstepState, FootstepGraph >::Ptr node, FootstepGraph::Ptr graph)
const Eigen::Vector3f footstep_size(0.2, 0.1, 0.000001)
static WallTime now()
int count
std::ofstream ofs("test.txt")
just a pointcloud generator for sample usage
double x
int main(int argc, char **argv)


jsk_footstep_planner
Author(s): Ryohei Ueda
autogenerated on Thu Nov 14 2019 03:53:28