footstep_planning_2d_demo.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 using namespace jsk_footstep_planner;
00042 
00043 const Eigen::Vector3f footstep_size(0.2, 0.1, 0.000001);
00044 const Eigen::Vector3f resolution(0.05, 0.05, 0.08);
00045 
00046 void plan(double x, double y, double yaw,
00047           FootstepGraph::Ptr graph, ros::Publisher& pub_path,
00048           ros::Publisher& pub_goal,
00049           Eigen::Vector3f footstep_size)
00050 {
00051   Eigen::Affine3f goal_center = affineFromXYYaw(x, y, yaw);
00052   FootstepState::Ptr left_goal(new FootstepState(jsk_footstep_msgs::Footstep::LEFT,
00053                                                  goal_center * Eigen::Translation3f(0, 0.1, 0),
00054                                                  footstep_size,
00055                                                  resolution));
00056   FootstepState::Ptr right_goal(new FootstepState(jsk_footstep_msgs::Footstep::RIGHT,
00057                                                   goal_center * Eigen::Translation3f(0, -0.1, 0),
00058                                                   footstep_size,
00059                                                   resolution));
00060 
00061   jsk_footstep_msgs::FootstepArray ros_goal;
00062   ros_goal.header.frame_id = "odom";
00063   ros_goal.header.stamp = ros::Time::now();
00064   ros_goal.footsteps.push_back(*left_goal->toROSMsg());
00065   ros_goal.footsteps.push_back(*right_goal->toROSMsg());
00066   pub_goal.publish(ros_goal);
00067     
00068   graph->setGoalState(left_goal, right_goal);
00069   //AStarSolver<FootstepGraph> solver(graph);
00070   FootstepAStarSolver<FootstepGraph> solver(graph, 100, 100, 100);
00071   //solver.setHeuristic(&footstepHeuristicStraight);
00072   //solver.setHeuristic(&footstepHeuristicStraightRotation);
00073   solver.setHeuristic(boost::bind(&footstepHeuristicStepCost, _1, _2, 1.0, 0.1));
00074   ros::WallTime start_time = ros::WallTime::now();
00075   std::vector<SolverNode<FootstepState, FootstepGraph>::Ptr> path = solver.solve();
00076   ros::WallTime end_time = ros::WallTime::now();
00077   std::cout << "took " << (end_time - start_time).toSec() << " sec" << std::endl;
00078   std::cout << "path: " << path.size() << std::endl;
00079   jsk_footstep_msgs::FootstepArray ros_path;
00080   ros_path.header.frame_id = "odom";
00081   ros_path.header.stamp = ros::Time::now();
00082   for (size_t i = 0; i < path.size(); i++) {
00083     ros_path.footsteps.push_back(*path[i]->getState()->toROSMsg());
00084   }
00085   pub_path.publish(ros_path);
00086 }
00087 
00088 int main(int argc, char** argv)
00089 {
00090   ros::init(argc, argv, "foootstep_planning_2d");
00091   ros::NodeHandle nh("~");
00092   ros::Publisher pub_start = nh.advertise<jsk_footstep_msgs::FootstepArray>("start", 1, true);
00093   ros::Publisher pub_goal = nh.advertise<jsk_footstep_msgs::FootstepArray>("goal", 1, true);
00094   ros::Publisher pub_path = nh.advertise<jsk_footstep_msgs::FootstepArray>("path", 1, true);
00095   boost::mt19937 rng( static_cast<unsigned long>(time(0)) );
00096   boost::uniform_real<> xyrange(-3.0,3.0);
00097   boost::variate_generator< boost::mt19937, boost::uniform_real<> > pos_rand(rng, xyrange);
00098   boost::uniform_real<> trange(0, 2 * M_PI);
00099   boost::variate_generator< boost::mt19937, boost::uniform_real<> > rot_rand(rng, trange);
00100 
00101   FootstepGraph::Ptr graph(new FootstepGraph(resolution));
00102   //graph->setProgressPublisher(nh, "progress");
00103   // set successors
00104   std::vector<Eigen::Affine3f> successors;
00105   successors.push_back(affineFromXYYaw(0, -0.2, 0));
00106   successors.push_back(affineFromXYYaw(0, -0.3, 0));
00107   successors.push_back(affineFromXYYaw(0, -0.15, 0));
00108   successors.push_back(affineFromXYYaw(0.2, -0.2, 0));
00109   successors.push_back(affineFromXYYaw(0.25, -0.2, 0));
00110   successors.push_back(affineFromXYYaw(0.1, -0.2, 0));
00111   successors.push_back(affineFromXYYaw(-0.1, -0.2, 0));
00112   successors.push_back(affineFromXYYaw(0, -0.2, 0.17));
00113   successors.push_back(affineFromXYYaw(0, -0.3, 0.17));
00114   successors.push_back(affineFromXYYaw(0.2, -0.2, 0.17));
00115   successors.push_back(affineFromXYYaw(0.25, -0.2, 0.17));
00116   successors.push_back(affineFromXYYaw(0.1, -0.2, 0.17));
00117   successors.push_back(affineFromXYYaw(0, -0.2, -0.17));
00118   successors.push_back(affineFromXYYaw(0, -0.3, -0.17));
00119   successors.push_back(affineFromXYYaw(0.2, -0.2, -0.17));
00120   successors.push_back(affineFromXYYaw(0.25, -0.2, -0.17));
00121   successors.push_back(affineFromXYYaw(0.1, -0.2, -0.17));
00122   FootstepState::Ptr start(new FootstepState(jsk_footstep_msgs::Footstep::LEFT,
00123                                              Eigen::Affine3f::Identity(),
00124                                              footstep_size,
00125                                              resolution));
00126   graph->setStartState(start);
00127   graph->setBasicSuccessors(successors);
00128   jsk_footstep_msgs::FootstepArray ros_start;
00129   ros_start.header.frame_id = "odom";
00130   ros_start.header.stamp = ros::Time::now();
00131   ros_start.footsteps.push_back(*start->toROSMsg());
00132   pub_start.publish(ros_start);
00133 
00134   plan(1, 1, 0, graph, pub_path, pub_goal, footstep_size);
00135   ros::Duration(1.0).sleep();
00136   plan(1, 2, 0, graph, pub_path, pub_goal, footstep_size);
00137   ros::Duration(1.0).sleep();
00138   plan(1, 1, 0.2, graph, pub_path, pub_goal, footstep_size);
00139   ros::Duration(1.0).sleep();
00140   plan(1, 0, 0, graph, pub_path, pub_goal, footstep_size);
00141   ros::Duration(1.0).sleep();
00142   plan(2, 0, 0, graph, pub_path, pub_goal, footstep_size);
00143   ros::Duration(1.0).sleep();
00144   plan(3, 0, 0, graph, pub_path, pub_goal, footstep_size);
00145   ros::Duration(1.0).sleep();
00146     
00147   while (ros::ok()) {
00148     plan(pos_rand(), pos_rand(), rot_rand(), graph, pub_path, pub_goal, footstep_size);
00149     ros::Duration(1.0).sleep();
00150   }
00151   return 0;
00152 }


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