footstep_planning_2d_demo.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 using namespace jsk_footstep_planner;
42 
43 const Eigen::Vector3f footstep_size(0.2, 0.1, 0.000001);
44 const Eigen::Vector3f resolution(0.05, 0.05, 0.08);
45 
46 void plan(double x, double y, double yaw,
49  Eigen::Vector3f footstep_size)
50 {
51  Eigen::Affine3f goal_center = affineFromXYYaw(x, y, yaw);
52  FootstepState::Ptr left_goal(new FootstepState(jsk_footstep_msgs::Footstep::LEFT,
53  goal_center * Eigen::Translation3f(0, 0.1, 0),
54  footstep_size,
55  resolution));
56  FootstepState::Ptr right_goal(new FootstepState(jsk_footstep_msgs::Footstep::RIGHT,
57  goal_center * Eigen::Translation3f(0, -0.1, 0),
58  footstep_size,
59  resolution));
60 
61  jsk_footstep_msgs::FootstepArray ros_goal;
62  ros_goal.header.frame_id = "odom";
63  ros_goal.header.stamp = ros::Time::now();
64  ros_goal.footsteps.push_back(*left_goal->toROSMsg());
65  ros_goal.footsteps.push_back(*right_goal->toROSMsg());
66  pub_goal.publish(ros_goal);
67 
68  graph->setGoalState(left_goal, right_goal);
69  //AStarSolver<FootstepGraph> solver(graph);
70  FootstepAStarSolver<FootstepGraph> solver(graph, 100, 100, 100);
71  //solver.setHeuristic(&footstepHeuristicStraight);
72  //solver.setHeuristic(&footstepHeuristicStraightRotation);
73  solver.setHeuristic(boost::bind(&footstepHeuristicStepCost, _1, _2, 1.0, 0.1));
74  ros::WallTime start_time = ros::WallTime::now();
75  std::vector<SolverNode<FootstepState, FootstepGraph>::Ptr> path = solver.solve();
76  ros::WallTime end_time = ros::WallTime::now();
77  std::cout << "took " << (end_time - start_time).toSec() << " sec" << std::endl;
78  std::cout << "path: " << path.size() << std::endl;
79  jsk_footstep_msgs::FootstepArray ros_path;
80  ros_path.header.frame_id = "odom";
81  ros_path.header.stamp = ros::Time::now();
82  for (size_t i = 0; i < path.size(); i++) {
83  ros_path.footsteps.push_back(*path[i]->getState()->toROSMsg());
84  }
85  pub_path.publish(ros_path);
86 }
87 
88 int main(int argc, char** argv)
89 {
90  ros::init(argc, argv, "foootstep_planning_2d");
91  ros::NodeHandle nh("~");
92  ros::Publisher pub_start = nh.advertise<jsk_footstep_msgs::FootstepArray>("start", 1, true);
93  ros::Publisher pub_goal = nh.advertise<jsk_footstep_msgs::FootstepArray>("goal", 1, true);
94  ros::Publisher pub_path = nh.advertise<jsk_footstep_msgs::FootstepArray>("path", 1, true);
95  boost::mt19937 rng( static_cast<unsigned long>(time(0)) );
96  boost::uniform_real<> xyrange(-3.0,3.0);
97  boost::variate_generator< boost::mt19937, boost::uniform_real<> > pos_rand(rng, xyrange);
98  boost::uniform_real<> trange(0, 2 * M_PI);
99  boost::variate_generator< boost::mt19937, boost::uniform_real<> > rot_rand(rng, trange);
100 
102  //graph->setProgressPublisher(nh, "progress");
103  // set successors
104  std::vector<Eigen::Affine3f> successors;
105  successors.push_back(affineFromXYYaw(0, -0.2, 0));
106  successors.push_back(affineFromXYYaw(0, -0.3, 0));
107  successors.push_back(affineFromXYYaw(0, -0.15, 0));
108  successors.push_back(affineFromXYYaw(0.2, -0.2, 0));
109  successors.push_back(affineFromXYYaw(0.25, -0.2, 0));
110  successors.push_back(affineFromXYYaw(0.1, -0.2, 0));
111  successors.push_back(affineFromXYYaw(-0.1, -0.2, 0));
112  successors.push_back(affineFromXYYaw(0, -0.2, 0.17));
113  successors.push_back(affineFromXYYaw(0, -0.3, 0.17));
114  successors.push_back(affineFromXYYaw(0.2, -0.2, 0.17));
115  successors.push_back(affineFromXYYaw(0.25, -0.2, 0.17));
116  successors.push_back(affineFromXYYaw(0.1, -0.2, 0.17));
117  successors.push_back(affineFromXYYaw(0, -0.2, -0.17));
118  successors.push_back(affineFromXYYaw(0, -0.3, -0.17));
119  successors.push_back(affineFromXYYaw(0.2, -0.2, -0.17));
120  successors.push_back(affineFromXYYaw(0.25, -0.2, -0.17));
121  successors.push_back(affineFromXYYaw(0.1, -0.2, -0.17));
122  FootstepState::Ptr start(new FootstepState(jsk_footstep_msgs::Footstep::LEFT,
123  Eigen::Affine3f::Identity(),
125  resolution));
126  graph->setStartState(start);
127  graph->setBasicSuccessors(successors);
128  jsk_footstep_msgs::FootstepArray ros_start;
129  ros_start.header.frame_id = "odom";
130  ros_start.header.stamp = ros::Time::now();
131  ros_start.footsteps.push_back(*start->toROSMsg());
132  pub_start.publish(ros_start);
133 
134  plan(1, 1, 0, graph, pub_path, pub_goal, footstep_size);
135  ros::Duration(1.0).sleep();
136  plan(1, 2, 0, graph, pub_path, pub_goal, footstep_size);
137  ros::Duration(1.0).sleep();
138  plan(1, 1, 0.2, graph, pub_path, pub_goal, footstep_size);
139  ros::Duration(1.0).sleep();
140  plan(1, 0, 0, graph, pub_path, pub_goal, footstep_size);
141  ros::Duration(1.0).sleep();
142  plan(2, 0, 0, graph, pub_path, pub_goal, footstep_size);
143  ros::Duration(1.0).sleep();
144  plan(3, 0, 0, graph, pub_path, pub_goal, footstep_size);
145  ros::Duration(1.0).sleep();
146 
147  while (ros::ok()) {
148  plan(pos_rand(), pos_rand(), rot_rand(), graph, pub_path, pub_goal, footstep_size);
149  ros::Duration(1.0).sleep();
150  }
151  return 0;
152 }
double footstepHeuristicStepCost(SolverNode< FootstepState, FootstepGraph >::Ptr node, FootstepGraph::Ptr graph, double first_rotation_weight, double second_rotation_weight)
ros::Publisher pub_goal
path
void publish(const boost::shared_ptr< M > &message) const
Eigen::Affine3f affineFromXYYaw(double x, double y, double yaw)
Definition: util.h:42
const Eigen::Vector3f footstep_size(0.2, 0.1, 0.000001)
bool sleep() const
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
#define M_PI
virtual std::vector< typename SolverNode< State, GraphT >::Ptr > solve(const ros::WallDuration &timeout=ros::WallDuration(1000000000.0))
const Eigen::Vector3f resolution(0.05, 0.05, 0.08)
virtual void setHeuristic(HeuristicFunction h)
Definition: astar_solver.h:71
ROSCPP_DECL bool ok()
FootstepGraph::Ptr graph
start
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
ros::Publisher pub_path
static WallTime now()
static Time now()
void plan(double x, double y, double yaw, FootstepGraph::Ptr graph, ros::Publisher &pub_path, ros::Publisher &pub_goal, Eigen::Vector3f footstep_size)
int main(int argc, char **argv)


jsk_footstep_planner
Author(s): Ryohei Ueda
autogenerated on Fri May 14 2021 02:51:52