jsk_footstep_planner: footstep_planning_2d_interactive

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 using namespace jsk_footstep_planner;
00047 
00048 const Eigen::Vector3f footstep_size(0.2, 0.1, 0.000001);
00049 const Eigen::Vector3f resolution(0.05, 0.05, 0.08);
00050 ros::Publisher pub_goal, pub_path, pub_text;
00051 FootstepGraph::Ptr graph;
00052 
00053 void profile(FootstepAStarSolver<FootstepGraph>& solver, FootstepGraph::Ptr graph)
00054 {
00055   ROS_INFO("open list: %lu", solver.getOpenList().size());
00056   ROS_INFO("close list: %lu", solver.getCloseList().size());
00057 }
00058 
00059 void plan(const Eigen::Affine3f& goal_center,
00060           FootstepGraph::Ptr graph, ros::Publisher& pub_path,
00061           ros::Publisher& pub_goal,
00062           Eigen::Vector3f footstep_size)
00063 {
00064   FootstepState::Ptr left_goal(new FootstepState(jsk_footstep_msgs::Footstep::LEFT,
00065                                                  goal_center * Eigen::Translation3f(0, 0.1, 0),
00066                                                  footstep_size,
00067                                                  resolution));
00068   FootstepState::Ptr right_goal(new FootstepState(jsk_footstep_msgs::Footstep::RIGHT,
00069                                                   goal_center * Eigen::Translation3f(0, -0.1, 0),
00070                                                   footstep_size,
00071                                                   resolution));
00072 
00073   jsk_footstep_msgs::FootstepArray ros_goal;
00074   ros_goal.header.frame_id = "odom";
00075   ros_goal.header.stamp = ros::Time::now();
00076   ros_goal.footsteps.push_back(*left_goal->toROSMsg());
00077   ros_goal.footsteps.push_back(*right_goal->toROSMsg());
00078   pub_goal.publish(ros_goal);
00079     
00080   graph->setGoalState(left_goal, right_goal);
00081   //AStarSolver<FootstepGraph> solver(graph);
00082   FootstepAStarSolver<FootstepGraph> solver(graph, 100, 100, 100);
00083   //solver.setHeuristic(&footstepHeuristicStraight);
00084   //solver.setHeuristic(&footstepHeuristicStraightRotation);
00085   solver.setHeuristic(boost::bind(&footstepHeuristicStepCost, _1, _2, 1.0, 0.1));
00086   solver.setProfileFunction(&profile);
00087   ros::WallTime start_time = ros::WallTime::now();
00088   std::vector<SolverNode<FootstepState, FootstepGraph>::Ptr> path = solver.solve();
00089   ros::WallTime end_time = ros::WallTime::now();
00090   std::cout << "took " << (end_time - start_time).toSec() << " sec" << std::endl;
00091   std::cout << "path: " << path.size() << std::endl;
00092   jsk_footstep_msgs::FootstepArray ros_path;
00093   ros_path.header.frame_id = "odom";
00094   ros_path.header.stamp = ros::Time::now();
00095   for (size_t i = 0; i < path.size(); i++) {
00096     ros_path.footsteps.push_back(*path[i]->getState()->toROSMsg());
00097   }
00098   pub_path.publish(ros_path);
00099 
00100 }
00101 
00102 
00103 void processFeedback(
00104     const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback )
00105 {
00106   Eigen::Affine3f new_pose;
00107   tf::poseMsgToEigen(feedback->pose, new_pose);
00108   ros::WallTime start = ros::WallTime::now();
00109   plan(new_pose, graph, pub_path, pub_goal, footstep_size);
00110   ros::WallTime end = ros::WallTime::now();
00111   jsk_rviz_plugins::OverlayText text;
00112   text.bg_color.a = 0.0;
00113   text.fg_color.a = 1.0;
00114   text.fg_color.r = 25 / 255.0;
00115   text.fg_color.g = 1.0;
00116   text.fg_color.b = 250 / 255.0;
00117   text.line_width = 2;
00118   text.top = 10;
00119   text.left = 10;
00120   text.text_size = 24;
00121   text.width = 600;
00122   text.height = 100;
00123   text.text = (boost::format("Planning took %f sec") % (end - start).toSec()).str();
00124   pub_text.publish(text);
00125 }
00126 
00127 int main(int argc, char** argv)
00128 {
00129   ros::init(argc, argv, "foootstep_planning_2d");
00130   ros::NodeHandle nh("~");
00131   ros::Publisher pub_start = nh.advertise<jsk_footstep_msgs::FootstepArray>("start", 1, true);
00132   pub_goal = nh.advertise<jsk_footstep_msgs::FootstepArray>("goal", 1, true);
00133   pub_path = nh.advertise<jsk_footstep_msgs::FootstepArray>("path", 1, true);
00134   pub_text = nh.advertise<jsk_rviz_plugins::OverlayText>("text", 1, true);
00135   boost::mt19937 rng( static_cast<unsigned long>(time(0)) );
00136   boost::uniform_real<> xyrange(-3.0,3.0);
00137   boost::variate_generator< boost::mt19937, boost::uniform_real<> > pos_rand(rng, xyrange);
00138   boost::uniform_real<> trange(0, 2 * M_PI);
00139   boost::variate_generator< boost::mt19937, boost::uniform_real<> > rot_rand(rng, trange);
00140 
00141   graph.reset(new FootstepGraph(resolution));
00142   //graph->setProgressPublisher(nh, "progress");
00143   // set successors
00144   std::vector<Eigen::Affine3f> successors;
00145   successors.push_back(affineFromXYYaw(0, -0.2, 0));
00146   successors.push_back(affineFromXYYaw(0, -0.3, 0));
00147   successors.push_back(affineFromXYYaw(0, -0.15, 0));
00148   successors.push_back(affineFromXYYaw(0.2, -0.2, 0));
00149   successors.push_back(affineFromXYYaw(0.25, -0.2, 0));
00150   successors.push_back(affineFromXYYaw(0.1, -0.2, 0));
00151   successors.push_back(affineFromXYYaw(-0.1, -0.2, 0));
00152   successors.push_back(affineFromXYYaw(0, -0.2, 0.17));
00153   successors.push_back(affineFromXYYaw(0, -0.3, 0.17));
00154   successors.push_back(affineFromXYYaw(0.2, -0.2, 0.17));
00155   successors.push_back(affineFromXYYaw(0.25, -0.2, 0.17));
00156   successors.push_back(affineFromXYYaw(0.1, -0.2, 0.17));
00157   successors.push_back(affineFromXYYaw(0, -0.2, -0.17));
00158   successors.push_back(affineFromXYYaw(0, -0.3, -0.17));
00159   successors.push_back(affineFromXYYaw(0.2, -0.2, -0.17));
00160   successors.push_back(affineFromXYYaw(0.25, -0.2, -0.17));
00161   successors.push_back(affineFromXYYaw(0.1, -0.2, -0.17));
00162   FootstepState::Ptr start(new FootstepState(jsk_footstep_msgs::Footstep::LEFT,
00163                                              Eigen::Affine3f::Identity(),
00164                                              footstep_size,
00165                                              resolution));
00166   graph->setStartState(start);
00167   graph->setBasicSuccessors(successors);
00168   jsk_footstep_msgs::FootstepArray ros_start;
00169   ros_start.header.frame_id = "odom";
00170   ros_start.header.stamp = ros::Time::now();
00171   ros_start.footsteps.push_back(*start->toROSMsg());
00172   pub_start.publish(ros_start);
00173   interactive_markers::InteractiveMarkerServer server("footstep_projection_demo");
00174 
00175   visualization_msgs::InteractiveMarker int_marker;
00176   int_marker.header.frame_id = "/odom";
00177   int_marker.header.stamp=ros::Time::now();
00178   int_marker.name = "footstep marker";
00179 
00180   visualization_msgs::InteractiveMarkerControl control;
00181 
00182   control.orientation.w = 1;
00183   control.orientation.x = 1;
00184   control.orientation.y = 0;
00185   control.orientation.z = 0;
00186   // control.interaction_mode = visualization_msgs::InteractiveMarkerControl::ROTATE_AXIS;
00187   // int_marker.controls.push_back(control);
00188   control.interaction_mode = visualization_msgs::InteractiveMarkerControl::MOVE_AXIS;
00189   int_marker.controls.push_back(control);
00190 
00191   control.orientation.w = 1;
00192   control.orientation.x = 0;
00193   control.orientation.y = 1;
00194   control.orientation.z = 0;
00195   control.interaction_mode = visualization_msgs::InteractiveMarkerControl::ROTATE_AXIS;
00196   int_marker.controls.push_back(control);
00197   // control.interaction_mode = visualization_msgs::InteractiveMarkerControl::MOVE_AXIS;
00198   // int_marker.controls.push_back(control);
00199 
00200   control.orientation.w = 1;
00201   control.orientation.x = 0;
00202   control.orientation.y = 0;
00203   control.orientation.z = 1;
00204   // control.interaction_mode = visualization_msgs::InteractiveMarkerControl::ROTATE_AXIS;
00205   // int_marker.controls.push_back(control);
00206   control.interaction_mode = visualization_msgs::InteractiveMarkerControl::MOVE_AXIS;
00207   int_marker.controls.push_back(control);
00208   server.insert(int_marker, &processFeedback);
00209   server.applyChanges();
00210   //plan(Eigen::Affine3f::Identity() * Eigen::Translation3f(3, 3, 0), graph, pub_path, pub_goal, footstep_size);
00211   ros::spin();
00212   return 0;
00213 }