footstep_planning_2d_perception_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 #include <interactive_markers/tools.h>
00042 #include <interactive_markers/interactive_marker_server.h>
00043 #include <jsk_recognition_utils/pcl_conversion_util.h>
00044 #include <jsk_rviz_plugins/OverlayText.h>
00045 #include <boost/format.hpp>
00046 #include <jsk_recognition_utils/pcl_conversion_util.h>
00047 
00048 using namespace jsk_footstep_planner;
00049 
00050 const Eigen::Vector3f footstep_size(0.2, 0.1, 0.000001);
00051 const Eigen::Vector3f resolution(0.05, 0.05, 0.08);
00052 ros::Publisher pub_goal, pub_path, pub_text, pub_close_list, pub_open_list;
00053 FootstepGraph::Ptr graph;
00054 
00055 void profile(FootstepAStarSolver<FootstepGraph>& solver, FootstepGraph::Ptr graph)
00056 {
00057   ROS_INFO("open list: %lu", solver.getOpenList().size());
00058   ROS_INFO("close list: %lu", solver.getCloseList().size());
00059   FootstepAStarSolver<FootstepGraph>::OpenList open_list = solver.getOpenList();
00060 }
00061 
00062 void plan(const Eigen::Affine3f& goal_center,
00063           FootstepGraph::Ptr graph, ros::Publisher& pub_path,
00064           ros::Publisher& pub_goal,
00065           Eigen::Vector3f footstep_size)
00066 {
00067   FootstepState::Ptr left_goal(new FootstepState(jsk_footstep_msgs::Footstep::LEFT,
00068                                                  goal_center * Eigen::Translation3f(0, 0.1, 0),
00069                                                  footstep_size,
00070                                                  resolution));
00071   FootstepState::Ptr right_goal(new FootstepState(jsk_footstep_msgs::Footstep::RIGHT,
00072                                                   goal_center * Eigen::Translation3f(0, -0.1, 0),
00073                                                   footstep_size,
00074                                                   resolution));
00075     
00076   graph->setGoalState(left_goal, right_goal);
00077   if (!graph->projectGoal()) {
00078     ROS_ERROR("Failed to project goal");
00079     return;
00080   }
00081 
00082   jsk_footstep_msgs::FootstepArray ros_goal;
00083   ros_goal.header.frame_id = "odom";
00084   ros_goal.header.stamp = ros::Time::now();
00085   ros_goal.footsteps.push_back(*graph->getGoal(jsk_footstep_msgs::Footstep::LEFT)->toROSMsg());
00086   ros_goal.footsteps.push_back(*graph->getGoal(jsk_footstep_msgs::Footstep::RIGHT)->toROSMsg());
00087   pub_goal.publish(ros_goal);
00088 
00089   
00090   //AStarSolver<FootstepGraph> solver(graph);
00091   FootstepAStarSolver<FootstepGraph> solver(graph, 100, 100, 100);
00092   //solver.setHeuristic(&footstepHeuristicStraight);
00093   //solver.setHeuristic(&footstepHeuristicStraightRotation);
00094   solver.setHeuristic(boost::bind(&footstepHeuristicStepCost, _1, _2, 1.0, 0.1));
00095   solver.setProfileFunction(&profile);
00096   ros::WallTime start_time = ros::WallTime::now();
00097   std::vector<SolverNode<FootstepState, FootstepGraph>::Ptr> path = solver.solve(ros::WallDuration(2000.0));
00098   ros::WallTime end_time = ros::WallTime::now();
00099   std::cout << "took " << (end_time - start_time).toSec() << " sec" << std::endl;
00100   std::cout << "path: " << path.size() << std::endl;
00101   if (path.size() == 0) {
00102     ROS_ERROR("Failed to plan path");
00103     return;
00104   }
00105   jsk_footstep_msgs::FootstepArray ros_path;
00106   ros_path.header.frame_id = "odom";
00107   ros_path.header.stamp = ros::Time::now();
00108   for (size_t i = 0; i < path.size(); i++) {
00109     ros_path.footsteps.push_back(*path[i]->getState()->toROSMsg());
00110   }
00111   pub_path.publish(ros_path);
00112   pcl::PointCloud<pcl::PointXYZ> close_cloud, open_cloud;
00113   solver.openListToPointCloud<pcl::PointXYZ>(open_cloud);
00114   solver.closeListToPointCloud<pcl::PointXYZ>(close_cloud);
00115   sensor_msgs::PointCloud2 ros_close_cloud, ros_open_cloud;
00116   pcl::toROSMsg(close_cloud, ros_close_cloud);
00117   ros_close_cloud.header.stamp = ros::Time::now();
00118   ros_close_cloud.header.frame_id = "odom";
00119   pub_close_list.publish(ros_close_cloud);
00120   pcl::toROSMsg(open_cloud, ros_open_cloud);
00121   ros_open_cloud.header.stamp = ros::Time::now();
00122   ros_open_cloud.header.frame_id = "odom";
00123   pub_open_list.publish(ros_open_cloud);
00124 }
00125 
00126 
00127 void processFeedback(
00128     const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback )
00129 {
00130   if (feedback->event_type != visualization_msgs::InteractiveMarkerFeedback::MOUSE_UP) {
00131     return;
00132   }
00133   Eigen::Affine3f new_pose;
00134   tf::poseMsgToEigen(feedback->pose, new_pose);
00135   ros::WallTime start = ros::WallTime::now();
00136   plan(new_pose, graph, pub_path, pub_goal, footstep_size);
00137   ros::WallTime end = ros::WallTime::now();
00138   jsk_rviz_plugins::OverlayText text;
00139   text.bg_color.a = 0.0;
00140   text.fg_color.a = 1.0;
00141   text.fg_color.r = 25 / 255.0;
00142   text.fg_color.g = 1.0;
00143   text.fg_color.b = 250 / 255.0;
00144   text.line_width = 2;
00145   text.top = 10;
00146   text.left = 10;
00147   text.text_size = 24;
00148   text.width = 600;
00149   text.height = 100;
00150   text.text = (boost::format("Planning took %f sec") % (end - start).toSec()).str();
00151   pub_text.publish(text);
00152 }
00153 
00154 pcl::PointCloud<pcl::PointNormal>::Ptr
00155 generateCloudSlope()
00156 {
00157   pcl::PointCloud<pcl::PointNormal>::Ptr gen_cloud(new pcl::PointCloud<pcl::PointNormal>);
00158   for (double y = -0.5; y < 0.5; y = y + 0.01) {
00159     for (double x = -1.0; x < 0.5; x = x + 0.01) {
00160       pcl::PointNormal p;
00161       p.x = x;
00162       p.y = y;
00163       gen_cloud->points.push_back(p);
00164     }
00165     for (double x = 0.5; x < 5.0; x = x + 0.01) {
00166       pcl::PointNormal p;
00167       p.x = x;
00168       p.y = y;
00169       p.z = 0.2 * x - 0.5 * 0.2;
00170       gen_cloud->points.push_back(p);
00171     }
00172   }
00173   return gen_cloud;
00174 }
00175 
00176 pcl::PointCloud<pcl::PointNormal>::Ptr
00177 generateCloudFlat()
00178 {
00179   pcl::PointCloud<pcl::PointNormal>::Ptr gen_cloud(new pcl::PointCloud<pcl::PointNormal>);
00180   for (double y = -2; y < 2; y = y + 0.01) {
00181     for (double x = -2; x < 2; x = x + 0.01) {
00182       pcl::PointNormal p;
00183       p.x = x;
00184       p.y = y;
00185       gen_cloud->points.push_back(p);
00186     }
00187   }
00188   return gen_cloud;
00189 }
00190 
00191 pcl::PointCloud<pcl::PointNormal>::Ptr
00192 generateCloudHills()
00193 {
00194   const double height = 0.1;
00195   pcl::PointCloud<pcl::PointNormal>::Ptr gen_cloud(new pcl::PointCloud<pcl::PointNormal>);
00196   for (double y = -2; y < 2; y = y + 0.01) {
00197     for (double x = -2; x < 2; x = x + 0.01) {
00198       pcl::PointNormal p;
00199       p.x = x;
00200       p.y = y;
00201       p.z = height * sin(x * 2) * sin(y * 2);
00202       gen_cloud->points.push_back(p);
00203     }
00204   }
00205   return gen_cloud;
00206 }
00207 
00208 pcl::PointCloud<pcl::PointNormal>::Ptr
00209 generateCloudStairs()
00210 {
00211   const double height = 0.1;
00212   pcl::PointCloud<pcl::PointNormal>::Ptr gen_cloud(new pcl::PointCloud<pcl::PointNormal>);
00213   for (double y = -2; y < 2; y = y + 0.01) {
00214     for (double x = -1; x < 0; x = x + 0.01) {
00215       pcl::PointNormal p;
00216       p.x = x;
00217       p.y = y;
00218       p.z = 0;
00219       gen_cloud->points.push_back(p);
00220     }
00221     for (double x = 0; x < 5; x = x + 0.01) {
00222       pcl::PointNormal p;
00223       p.x = x;
00224       p.y = y;
00225       p.z = floor(x * 4) * 0.1;
00226       gen_cloud->points.push_back(p);
00227     }
00228   }
00229   return gen_cloud;
00230 }
00231 
00232 
00233 int main(int argc, char** argv)
00234 {
00235   ros::init(argc, argv, "foootstep_planning_2d");
00236   ros::NodeHandle nh("~");
00237   ros::Publisher pub_start = nh.advertise<jsk_footstep_msgs::FootstepArray>("start", 1, true);
00238   pub_goal = nh.advertise<jsk_footstep_msgs::FootstepArray>("goal", 1, true);
00239   pub_path = nh.advertise<jsk_footstep_msgs::FootstepArray>("path", 1, true);
00240   pub_text = nh.advertise<jsk_rviz_plugins::OverlayText>("text", 1, true);
00241   ros::Publisher pub_cloud
00242     = nh.advertise<sensor_msgs::PointCloud2>("cloud", 1, true);
00243   pub_close_list
00244     = nh.advertise<sensor_msgs::PointCloud2>("close_list", 1, true);
00245   pub_open_list
00246     = nh.advertise<sensor_msgs::PointCloud2>("open_list", 1, true);
00247   boost::mt19937 rng( static_cast<unsigned long>(time(0)) );
00248   boost::uniform_real<> xyrange(-3.0,3.0);
00249   boost::variate_generator< boost::mt19937, boost::uniform_real<> > pos_rand(rng, xyrange);
00250   boost::uniform_real<> trange(0, 2 * M_PI);
00251   boost::variate_generator< boost::mt19937, boost::uniform_real<> > rot_rand(rng, trange);
00252   pcl::PointCloud<pcl::PointNormal>::Ptr cloud;
00253   std::string model;
00254   nh.param("model", model, std::string("flat"));
00255   if (model == "flat") {
00256     cloud = generateCloudFlat();
00257   }
00258   else if (model == "slope") {
00259     cloud = generateCloudSlope();
00260   }
00261   else if (model == "hills") {
00262     cloud = generateCloudHills();
00263   }
00264   else if (model == "stairs") {
00265     cloud = generateCloudStairs();
00266   }
00267   graph.reset(new FootstepGraph(resolution, true, true, true));
00268   sensor_msgs::PointCloud2 ros_cloud;
00269   pcl::toROSMsg(*cloud, ros_cloud);
00270   ros_cloud.header.frame_id = "odom";
00271   ros_cloud.header.stamp = ros::Time::now();
00272   pub_cloud.publish(ros_cloud);
00273   graph->setPointCloudModel(cloud);
00274   //graph->setProgressPublisher(nh, "progress");
00275   // set successors
00276   std::vector<Eigen::Affine3f> successors;
00277   successors.push_back(affineFromXYYaw(0, -0.2, 0));
00278   successors.push_back(affineFromXYYaw(0, -0.3, 0));
00279   successors.push_back(affineFromXYYaw(0, -0.15, 0));
00280   successors.push_back(affineFromXYYaw(0.2, -0.2, 0));
00281   successors.push_back(affineFromXYYaw(0.25, -0.2, 0));
00282   successors.push_back(affineFromXYYaw(0.3, -0.2, 0));
00283   successors.push_back(affineFromXYYaw(0.1, -0.2, 0));
00284   successors.push_back(affineFromXYYaw(-0.1, -0.2, 0));
00285   successors.push_back(affineFromXYYaw(0, -0.2, 0.17));
00286   successors.push_back(affineFromXYYaw(0, -0.3, 0.17));
00287   successors.push_back(affineFromXYYaw(0.2, -0.2, 0.17));
00288   successors.push_back(affineFromXYYaw(0.25, -0.2, 0.17));
00289   successors.push_back(affineFromXYYaw(0.1, -0.2, 0.17));
00290   successors.push_back(affineFromXYYaw(0, -0.2, -0.17));
00291   successors.push_back(affineFromXYYaw(0, -0.3, -0.17));
00292   successors.push_back(affineFromXYYaw(0.2, -0.2, -0.17));
00293   successors.push_back(affineFromXYYaw(0.25, -0.2, -0.17));
00294   successors.push_back(affineFromXYYaw(0.1, -0.2, -0.17));
00295   FootstepState::Ptr start(new FootstepState(jsk_footstep_msgs::Footstep::LEFT,
00296                                              Eigen::Affine3f::Identity(),
00297                                              footstep_size,
00298                                              resolution));
00299   graph->setStartState(start);
00300   graph->setBasicSuccessors(successors);
00301   jsk_footstep_msgs::FootstepArray ros_start;
00302   ros_start.header.frame_id = "odom";
00303   ros_start.header.stamp = ros::Time::now();
00304   ros_start.footsteps.push_back(*start->toROSMsg());
00305   pub_start.publish(ros_start);
00306   interactive_markers::InteractiveMarkerServer server("footstep_projection_demo");
00307 
00308     visualization_msgs::InteractiveMarker int_marker;
00309   int_marker.header.frame_id = "/odom";
00310   int_marker.header.stamp=ros::Time::now();
00311   int_marker.name = "footstep marker";
00312 
00313   visualization_msgs::InteractiveMarkerControl control;
00314 
00315   control.orientation.w = 1;
00316   control.orientation.x = 1;
00317   control.orientation.y = 0;
00318   control.orientation.z = 0;
00319   // control.interaction_mode = visualization_msgs::InteractiveMarkerControl::ROTATE_AXIS;
00320   // int_marker.controls.push_back(control);
00321   control.interaction_mode = visualization_msgs::InteractiveMarkerControl::MOVE_AXIS;
00322   int_marker.controls.push_back(control);
00323 
00324   control.orientation.w = 1;
00325   control.orientation.x = 0;
00326   control.orientation.y = 1;
00327   control.orientation.z = 0;
00328   control.interaction_mode = visualization_msgs::InteractiveMarkerControl::ROTATE_AXIS;
00329   int_marker.controls.push_back(control);
00330   control.interaction_mode = visualization_msgs::InteractiveMarkerControl::MOVE_AXIS;
00331   int_marker.controls.push_back(control);
00332 
00333   control.orientation.w = 1;
00334   control.orientation.x = 0;
00335   control.orientation.y = 0;
00336   control.orientation.z = 1;
00337   // control.interaction_mode = visualization_msgs::InteractiveMarkerControl::ROTATE_AXIS;
00338   // int_marker.controls.push_back(control);
00339   control.interaction_mode = visualization_msgs::InteractiveMarkerControl::MOVE_AXIS;
00340   int_marker.controls.push_back(control);
00341   server.insert(int_marker, &processFeedback);
00342   server.applyChanges();
00343   // std::cout << "-0.3 => " << int(-0.3) << ", " << int(floor(-0.3)) << std::endl;
00344   plan(Eigen::Affine3f::Identity() * Eigen::Translation3f(1.7, 0.0, 0), graph, pub_path, pub_goal, footstep_size);
00345   
00346   ros::spin();
00347   return 0;
00348 }


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