footstep_planning_2d_perception_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>
44 #include <jsk_rviz_plugins/OverlayText.h>
45 #include <boost/format.hpp>
47 
48 using namespace jsk_footstep_planner;
49 
50 const Eigen::Vector3f footstep_size(0.2, 0.1, 0.000001);
51 const Eigen::Vector3f resolution(0.05, 0.05, 0.08);
54 
56 {
57  ROS_INFO("open list: %lu", solver.getOpenList().size());
58  ROS_INFO("close list: %lu", solver.getCloseList().size());
60 }
61 
62 void plan(const Eigen::Affine3f& goal_center,
63  FootstepGraph::Ptr graph, ros::Publisher& pub_path,
64  ros::Publisher& pub_goal,
65  Eigen::Vector3f footstep_size)
66 {
67  FootstepState::Ptr left_goal(new FootstepState(jsk_footstep_msgs::Footstep::LEFT,
68  goal_center * Eigen::Translation3f(0, 0.1, 0),
69  footstep_size,
70  resolution));
71  FootstepState::Ptr right_goal(new FootstepState(jsk_footstep_msgs::Footstep::RIGHT,
72  goal_center * Eigen::Translation3f(0, -0.1, 0),
73  footstep_size,
74  resolution));
75 
76  graph->setGoalState(left_goal, right_goal);
77  if (!graph->projectGoal()) {
78  ROS_ERROR("Failed to project goal");
79  return;
80  }
81 
82  jsk_footstep_msgs::FootstepArray ros_goal;
83  ros_goal.header.frame_id = "odom";
84  ros_goal.header.stamp = ros::Time::now();
85  ros_goal.footsteps.push_back(*graph->getGoal(jsk_footstep_msgs::Footstep::LEFT)->toROSMsg());
86  ros_goal.footsteps.push_back(*graph->getGoal(jsk_footstep_msgs::Footstep::RIGHT)->toROSMsg());
87  pub_goal.publish(ros_goal);
88 
89 
90  //AStarSolver<FootstepGraph> solver(graph);
91  FootstepAStarSolver<FootstepGraph> solver(graph, 100, 100, 100);
92  //solver.setHeuristic(&footstepHeuristicStraight);
93  //solver.setHeuristic(&footstepHeuristicStraightRotation);
94  solver.setHeuristic(boost::bind(&footstepHeuristicStepCost, _1, _2, 1.0, 0.1));
95  solver.setProfileFunction(&profile);
96  ros::WallTime start_time = ros::WallTime::now();
97  std::vector<SolverNode<FootstepState, FootstepGraph>::Ptr> path = solver.solve(ros::WallDuration(2000.0));
98  ros::WallTime end_time = ros::WallTime::now();
99  std::cout << "took " << (end_time - start_time).toSec() << " sec" << std::endl;
100  std::cout << "path: " << path.size() << std::endl;
101  if (path.size() == 0) {
102  ROS_ERROR("Failed to plan path");
103  return;
104  }
105  jsk_footstep_msgs::FootstepArray ros_path;
106  ros_path.header.frame_id = "odom";
107  ros_path.header.stamp = ros::Time::now();
108  for (size_t i = 0; i < path.size(); i++) {
109  ros_path.footsteps.push_back(*path[i]->getState()->toROSMsg());
110  }
111  pub_path.publish(ros_path);
112  pcl::PointCloud<pcl::PointXYZ> close_cloud, open_cloud;
113  solver.openListToPointCloud<pcl::PointXYZ>(open_cloud);
114  solver.closeListToPointCloud<pcl::PointXYZ>(close_cloud);
115  sensor_msgs::PointCloud2 ros_close_cloud, ros_open_cloud;
116  pcl::toROSMsg(close_cloud, ros_close_cloud);
117  ros_close_cloud.header.stamp = ros::Time::now();
118  ros_close_cloud.header.frame_id = "odom";
119  pub_close_list.publish(ros_close_cloud);
120  pcl::toROSMsg(open_cloud, ros_open_cloud);
121  ros_open_cloud.header.stamp = ros::Time::now();
122  ros_open_cloud.header.frame_id = "odom";
123  pub_open_list.publish(ros_open_cloud);
124 }
125 
126 
128  const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback )
129 {
130  if (feedback->event_type != visualization_msgs::InteractiveMarkerFeedback::MOUSE_UP) {
131  return;
132  }
133  Eigen::Affine3f new_pose;
134  tf::poseMsgToEigen(feedback->pose, new_pose);
136  plan(new_pose, graph, pub_path, pub_goal, footstep_size);
138  jsk_rviz_plugins::OverlayText text;
139  text.bg_color.a = 0.0;
140  text.fg_color.a = 1.0;
141  text.fg_color.r = 25 / 255.0;
142  text.fg_color.g = 1.0;
143  text.fg_color.b = 250 / 255.0;
144  text.line_width = 2;
145  text.top = 10;
146  text.left = 10;
147  text.text_size = 24;
148  text.width = 600;
149  text.height = 100;
150  text.text = (boost::format("Planning took %f sec") % (end - start).toSec()).str();
151  pub_text.publish(text);
152 }
153 
154 pcl::PointCloud<pcl::PointNormal>::Ptr
156 {
157  pcl::PointCloud<pcl::PointNormal>::Ptr gen_cloud(new pcl::PointCloud<pcl::PointNormal>);
158  for (double y = -0.5; y < 0.5; y = y + 0.01) {
159  for (double x = -1.0; x < 0.5; x = x + 0.01) {
160  pcl::PointNormal p;
161  p.x = x;
162  p.y = y;
163  gen_cloud->points.push_back(p);
164  }
165  for (double x = 0.5; x < 5.0; x = x + 0.01) {
166  pcl::PointNormal p;
167  p.x = x;
168  p.y = y;
169  p.z = 0.2 * x - 0.5 * 0.2;
170  gen_cloud->points.push_back(p);
171  }
172  }
173  return gen_cloud;
174 }
175 
176 pcl::PointCloud<pcl::PointNormal>::Ptr
178 {
179  pcl::PointCloud<pcl::PointNormal>::Ptr gen_cloud(new pcl::PointCloud<pcl::PointNormal>);
180  for (double y = -2; y < 2; y = y + 0.01) {
181  for (double x = -2; x < 2; x = x + 0.01) {
182  pcl::PointNormal p;
183  p.x = x;
184  p.y = y;
185  gen_cloud->points.push_back(p);
186  }
187  }
188  return gen_cloud;
189 }
190 
191 pcl::PointCloud<pcl::PointNormal>::Ptr
193 {
194  const double height = 0.1;
195  pcl::PointCloud<pcl::PointNormal>::Ptr gen_cloud(new pcl::PointCloud<pcl::PointNormal>);
196  for (double y = -2; y < 2; y = y + 0.01) {
197  for (double x = -2; x < 2; x = x + 0.01) {
198  pcl::PointNormal p;
199  p.x = x;
200  p.y = y;
201  p.z = height * sin(x * 2) * sin(y * 2);
202  gen_cloud->points.push_back(p);
203  }
204  }
205  return gen_cloud;
206 }
207 
208 pcl::PointCloud<pcl::PointNormal>::Ptr
210 {
211  const double height = 0.1;
212  pcl::PointCloud<pcl::PointNormal>::Ptr gen_cloud(new pcl::PointCloud<pcl::PointNormal>);
213  for (double y = -2; y < 2; y = y + 0.01) {
214  for (double x = -1; x < 0; x = x + 0.01) {
215  pcl::PointNormal p;
216  p.x = x;
217  p.y = y;
218  p.z = 0;
219  gen_cloud->points.push_back(p);
220  }
221  for (double x = 0; x < 5; x = x + 0.01) {
222  pcl::PointNormal p;
223  p.x = x;
224  p.y = y;
225  p.z = floor(x * 4) * 0.1;
226  gen_cloud->points.push_back(p);
227  }
228  }
229  return gen_cloud;
230 }
231 
232 
233 int main(int argc, char** argv)
234 {
235  ros::init(argc, argv, "foootstep_planning_2d");
236  ros::NodeHandle nh("~");
237  ros::Publisher pub_start = nh.advertise<jsk_footstep_msgs::FootstepArray>("start", 1, true);
238  pub_goal = nh.advertise<jsk_footstep_msgs::FootstepArray>("goal", 1, true);
239  pub_path = nh.advertise<jsk_footstep_msgs::FootstepArray>("path", 1, true);
240  pub_text = nh.advertise<jsk_rviz_plugins::OverlayText>("text", 1, true);
242  = nh.advertise<sensor_msgs::PointCloud2>("cloud", 1, true);
243  pub_close_list
244  = nh.advertise<sensor_msgs::PointCloud2>("close_list", 1, true);
245  pub_open_list
246  = nh.advertise<sensor_msgs::PointCloud2>("open_list", 1, true);
247  boost::mt19937 rng( static_cast<unsigned long>(time(0)) );
248  boost::uniform_real<> xyrange(-3.0,3.0);
249  boost::variate_generator< boost::mt19937, boost::uniform_real<> > pos_rand(rng, xyrange);
250  boost::uniform_real<> trange(0, 2 * M_PI);
251  boost::variate_generator< boost::mt19937, boost::uniform_real<> > rot_rand(rng, trange);
252  pcl::PointCloud<pcl::PointNormal>::Ptr cloud;
253  std::string model;
254  nh.param("model", model, std::string("flat"));
255  if (model == "flat") {
256  cloud = generateCloudFlat();
257  }
258  else if (model == "slope") {
259  cloud = generateCloudSlope();
260  }
261  else if (model == "hills") {
262  cloud = generateCloudHills();
263  }
264  else if (model == "stairs") {
265  cloud = generateCloudStairs();
266  }
267  graph.reset(new FootstepGraph(resolution, true, true, true));
268  sensor_msgs::PointCloud2 ros_cloud;
269  pcl::toROSMsg(*cloud, ros_cloud);
270  ros_cloud.header.frame_id = "odom";
271  ros_cloud.header.stamp = ros::Time::now();
272  pub_cloud.publish(ros_cloud);
273  graph->setPointCloudModel(cloud);
274  //graph->setProgressPublisher(nh, "progress");
275  // set successors
276  std::vector<Eigen::Affine3f> successors;
277  successors.push_back(affineFromXYYaw(0, -0.2, 0));
278  successors.push_back(affineFromXYYaw(0, -0.3, 0));
279  successors.push_back(affineFromXYYaw(0, -0.15, 0));
280  successors.push_back(affineFromXYYaw(0.2, -0.2, 0));
281  successors.push_back(affineFromXYYaw(0.25, -0.2, 0));
282  successors.push_back(affineFromXYYaw(0.3, -0.2, 0));
283  successors.push_back(affineFromXYYaw(0.1, -0.2, 0));
284  successors.push_back(affineFromXYYaw(-0.1, -0.2, 0));
285  successors.push_back(affineFromXYYaw(0, -0.2, 0.17));
286  successors.push_back(affineFromXYYaw(0, -0.3, 0.17));
287  successors.push_back(affineFromXYYaw(0.2, -0.2, 0.17));
288  successors.push_back(affineFromXYYaw(0.25, -0.2, 0.17));
289  successors.push_back(affineFromXYYaw(0.1, -0.2, 0.17));
290  successors.push_back(affineFromXYYaw(0, -0.2, -0.17));
291  successors.push_back(affineFromXYYaw(0, -0.3, -0.17));
292  successors.push_back(affineFromXYYaw(0.2, -0.2, -0.17));
293  successors.push_back(affineFromXYYaw(0.25, -0.2, -0.17));
294  successors.push_back(affineFromXYYaw(0.1, -0.2, -0.17));
295  FootstepState::Ptr start(new FootstepState(jsk_footstep_msgs::Footstep::LEFT,
296  Eigen::Affine3f::Identity(),
298  resolution));
299  graph->setStartState(start);
300  graph->setBasicSuccessors(successors);
301  jsk_footstep_msgs::FootstepArray ros_start;
302  ros_start.header.frame_id = "odom";
303  ros_start.header.stamp = ros::Time::now();
304  ros_start.footsteps.push_back(*start->toROSMsg());
305  pub_start.publish(ros_start);
306  interactive_markers::InteractiveMarkerServer server("footstep_projection_demo");
307 
308  visualization_msgs::InteractiveMarker int_marker;
309  int_marker.header.frame_id = "/odom";
310  int_marker.header.stamp=ros::Time::now();
311  int_marker.name = "footstep marker";
312 
313  visualization_msgs::InteractiveMarkerControl control;
314 
315  control.orientation.w = 1;
316  control.orientation.x = 1;
317  control.orientation.y = 0;
318  control.orientation.z = 0;
319  // control.interaction_mode = visualization_msgs::InteractiveMarkerControl::ROTATE_AXIS;
320  // int_marker.controls.push_back(control);
321  control.interaction_mode = visualization_msgs::InteractiveMarkerControl::MOVE_AXIS;
322  int_marker.controls.push_back(control);
323 
324  control.orientation.w = 1;
325  control.orientation.x = 0;
326  control.orientation.y = 1;
327  control.orientation.z = 0;
328  control.interaction_mode = visualization_msgs::InteractiveMarkerControl::ROTATE_AXIS;
329  int_marker.controls.push_back(control);
330  control.interaction_mode = visualization_msgs::InteractiveMarkerControl::MOVE_AXIS;
331  int_marker.controls.push_back(control);
332 
333  control.orientation.w = 1;
334  control.orientation.x = 0;
335  control.orientation.y = 0;
336  control.orientation.z = 1;
337  // control.interaction_mode = visualization_msgs::InteractiveMarkerControl::ROTATE_AXIS;
338  // int_marker.controls.push_back(control);
339  control.interaction_mode = visualization_msgs::InteractiveMarkerControl::MOVE_AXIS;
340  int_marker.controls.push_back(control);
341  server.insert(int_marker, &processFeedback);
342  server.applyChanges();
343  // std::cout << "-0.3 => " << int(-0.3) << ", " << int(floor(-0.3)) << std::endl;
344  plan(Eigen::Affine3f::Identity() * Eigen::Translation3f(1.7, 0.0, 0), graph, pub_path, pub_goal, footstep_size);
345 
346  ros::spin();
347  return 0;
348 }
pcl::PointCloud< pcl::PointNormal >::Ptr generateCloudHills()
virtual void setProfileFunction(ProfileFunction f)
double footstepHeuristicStepCost(SolverNode< FootstepState, FootstepGraph >::Ptr node, FootstepGraph::Ptr graph, double first_rotation_weight, double second_rotation_weight)
path
void publish(const boost::shared_ptr< M > &message) const
const Eigen::Vector3f footstep_size(0.2, 0.1, 0.000001)
end
double sin()
Eigen::Affine3f affineFromXYYaw(double x, double y, double yaw)
Definition: util.h:42
void poseMsgToEigen(const geometry_msgs::Pose &m, Eigen::Affine3d &e)
void processFeedback(const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback)
ros::Publisher pub_close_list
FootstepGraph::Ptr graph
ros::Publisher pub_path
virtual FootstepStateDiscreteCloseList getCloseList()
void closeListToPointCloud(pcl::PointCloud< PointT > &output_cloud)
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
pcl::PointCloud< pcl::PointNormal >::Ptr generateCloudStairs()
server
void openListToPointCloud(pcl::PointCloud< PointT > &output_cloud)
text
double y
std::string model
ROSCPP_DECL void spin(Spinner &spinner)
ros::Publisher pub_text
#define M_PI
virtual std::vector< typename SolverNode< State, GraphT >::Ptr > solve(const ros::WallDuration &timeout=ros::WallDuration(1000000000.0))
pcl::PointCloud< pcl::PointNormal >::Ptr cloud
pcl::PointCloud< pcl::PointNormal >::Ptr generateCloudFlat()
virtual void setHeuristic(HeuristicFunction h)
Definition: astar_solver.h:71
#define ROS_INFO(...)
bool param(const std::string &param_name, T &param_val, const T &default_val) const
string str
pcl::PointCloud< pcl::PointNormal >::Ptr generateCloudSlope()
start
int main(int argc, char **argv)
void profile(FootstepAStarSolver< FootstepGraph > &solver, FootstepGraph::Ptr graph)
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
ros::Publisher pub_cloud
ros::Publisher pub_goal
void plan(const Eigen::Affine3f &goal_center, FootstepGraph::Ptr graph, ros::Publisher &pub_path, ros::Publisher &pub_goal, Eigen::Vector3f footstep_size)
void toROSMsg(const pcl::PointCloud< T > &cloud, sensor_msgs::Image &msg)
void insert(const visualization_msgs::InteractiveMarker &int_marker)
static WallTime now()
height
std::priority_queue< SolverNodePtr, std::vector< SolverNodePtr >, std::greater< SolverNodePtr > > OpenList
static Time now()
const Eigen::Vector3f resolution(0.05, 0.05, 0.08)
double x
#define ROS_ERROR(...)
ros::Publisher pub_open_list


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