rrt_demo.cpp
Go to the documentation of this file.
00001 /*********************************************************************
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Copyright (c) 2015, University of Colorado, Boulder
00005  *  Copyright (c) 2012, Willow Garage, Inc.
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/or other materials provided
00017  *     with the distribution.
00018  *   * Neither the name of the Willow Garage 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 
00036 /*
00037   Author: Dave Coleman <dave@dav.ee>
00038   Desc:   Visualize planning with OMPL in Rviz
00039 */
00040 
00041 // ROS
00042 #include <ros/ros.h>
00043 #include <ros/package.h> // for getting file path for loading images
00044 
00045 // Display in Rviz tool
00046 #include <ompl_visual_tools/ompl_visual_tools.h>
00047 #include <ompl_visual_tools/costs/two_dimensional_validity_checker.h>
00048 
00049 // OMPL
00050 #include <ompl/geometric/SimpleSetup.h>
00051 #include <ompl/geometric/planners/rrt/RRT.h>
00052 #include <ompl/geometric/planners/rrt/TRRT.h>
00053 #include <ompl/geometric/planners/rrt/RRTstar.h>
00054 #include <ompl/geometric/planners/rrt/RRTConnect.h>
00055 #include <ompl/base/PlannerTerminationCondition.h>
00056 #include <ompl/base/objectives/PathLengthOptimizationObjective.h>
00057 
00058 // Boost
00059 #include <boost/pointer_cast.hpp>
00060 
00061 namespace ob = ompl::base;
00062 namespace og = ompl::geometric;
00063 
00064 namespace ompl_visual_tools
00065 {
00066 
00067 static const std::string BASE_FRAME = "/world";
00068 
00072 class RRTDemo
00073 {
00074 
00075 private:
00076 
00077   og::SimpleSetupPtr simple_setup_;
00078 
00079   // Cost in 2D
00080   ompl::base::CostMap2DOptimizationObjectivePtr cost_map_;
00081 
00082   // Cost is just path length
00083   boost::shared_ptr<ompl::base::PathLengthOptimizationObjective> path_length_objective_;
00084 
00085   // The visual tools for interfacing with Rviz
00086   ompl_visual_tools::OmplVisualToolsPtr visual_tools_;
00087 
00088   // The number of dimensions - always 2 for images
00089   static const unsigned int DIMENSIONS = 2;
00090 
00091   // The state space we plan in
00092   ob::StateSpacePtr space_;
00093 
00094   // Remember what space we are working in
00095   ompl::base::SpaceInformationPtr si_;
00096 
00097   // Flag for determining amount of debug output to show
00098   bool verbose_;
00099 
00100   // Display graphics in Rviz
00101   bool use_visuals_;
00102 
00103 public:
00104 
00108   RRTDemo(bool verbose, bool use_visuals)
00109     : verbose_(verbose),
00110       use_visuals_(use_visuals)
00111   {
00112     // Construct the state space we are planning in
00113     space_.reset( new ob::RealVectorStateSpace( DIMENSIONS ));
00114 
00115     // Define an experience setup class
00116     simple_setup_ = og::SimpleSetupPtr( new og::SimpleSetup(space_) );
00117     si_ = simple_setup_->getSpaceInformation();
00118 
00119     // Load the tool for displaying in Rviz
00120     visual_tools_.reset(new ompl_visual_tools::OmplVisualTools(BASE_FRAME));
00121     visual_tools_->setSpaceInformation(si_);
00122     visual_tools_->setGlobalScale(100);
00123 
00124     // Set the planner
00125     //simple_setup_->setPlanner(ob::PlannerPtr(new og::RRTstar( si_ )));
00126     simple_setup_->setPlanner(ob::PlannerPtr(new og::RRT( si_ )));
00127 
00128     // Load the cost map
00129     cost_map_.reset(new ompl::base::CostMap2DOptimizationObjective( si_ ));
00130 
00131     // Load an alternitive optimization objective
00132     path_length_objective_.reset(new ompl::base::PathLengthOptimizationObjective( si_ ));
00133   }
00134 
00138   ~RRTDemo()
00139   {
00140   }
00141 
00145   void deleteAllMakers()
00146   {
00147     // Clear current rviz makers
00148     visual_tools_->deleteAllMarkers();
00149   }
00150 
00156   void loadCostMapImage( std::string image_path, double max_cost_threshold_percent = 0.4 )
00157   {
00158     cost_map_->max_cost_threshold_percent_ = max_cost_threshold_percent;
00159     cost_map_->loadImage(image_path);
00160 
00161     // Set the bounds for the R^2
00162     ob::RealVectorBounds bounds( DIMENSIONS );
00163     bounds.setLow( 0 ); // both dimensions start at 0
00164     bounds.setHigh( 0, cost_map_->image_->x - 1 ); // allow for non-square images
00165     bounds.setHigh( 1, cost_map_->image_->y - 1 ); // allow for non-square images
00166     space_->as<ob::RealVectorStateSpace>()->setBounds( bounds );
00167     space_->setup();
00168 
00169     // Pass cost to visualizer
00170     visual_tools_->setCostMap(cost_map_->cost_);
00171 
00172     // Set state validity checking for this space
00173     simple_setup_->setStateValidityChecker( ob::StateValidityCheckerPtr(
00174       new ob::TwoDimensionalValidityChecker( si_, cost_map_->cost_, cost_map_->max_cost_threshold_ ) ) );
00175 
00176     // The interval in which obstacles are checked for between states
00177     // seems that it default to 0.01 but doesn't do a good job at that level
00178     si_->setStateValidityCheckingResolution(0.001);
00179 
00180     // Setup the optimization objective to use the 2d cost map
00181     // NEWER VERSION OF OMPL
00182     //simple_setup_->setOptimizationObjective(cost_map_);
00183     //simple_setup_->setOptimizationObjective(path_length_objective_);
00184 
00185     // Setup -----------------------------------------------------------
00186 
00187     // Auto setup parameters
00188     simple_setup_->setup(); // optional
00189 
00190     //ROS_ERROR_STREAM_NAMED("temp","out of curiosity: coll check resolution: "
00191     //   << si_->getStateValidityCheckingResolution());
00192 
00193     // Debug - this call is optional, but we put it in to get more output information
00194     //simple_setup_->print();
00195   }
00196 
00197   void publishCostMapImage()
00198   {
00199     if (use_visuals_)
00200       visual_tools_->publishCostMap(cost_map_->image_);
00201   }
00202 
00209   bool plan(const int& run_id, const int& runs)
00210   {
00211     // Start and Goal State ---------------------------------------------
00212     ob::PlannerStatus solved;
00213 
00214     // Clear all planning data. This only includes data generated by motion plan computation.
00215     // Planner settings, start & goal states are not affected.
00216     if (run_id) // skip first run
00217       simple_setup_->clear();
00218 
00219     // Clear the previous solutions
00220     //simple_setup_->getProblemDefinition()->clearSolutionPaths();
00221 
00222     // Create the termination condition
00223     double seconds = 2;
00224     ob::PlannerTerminationCondition ptc = ob::timedPlannerTerminationCondition( seconds, 0.1 );
00225 
00226     // Create start and goal space
00227     ob::ScopedState<> start(space_);
00228     ob::ScopedState<> goal(space_);
00229     chooseStartGoal(start, goal);
00230 
00231     // Show start and goal
00232     if (use_visuals_)
00233     {
00234       visual_tools_->publishState(start, rviz_visual_tools::GREEN,  rviz_visual_tools::XLARGE, "plan_start_goal");
00235       visual_tools_->publishState(goal,  rviz_visual_tools::ORANGE, rviz_visual_tools::XLARGE, "plan_start_goal");
00236     }
00237 
00238     // set the start and goal states
00239     simple_setup_->setStartAndGoalStates(start, goal);
00240 
00241     // Solve -----------------------------------------------------------
00242 
00243     // attempt to solve the problem within x seconds of planning time
00244     solved = simple_setup_->solve( ptc );
00245 
00246     geometry_msgs::Pose text_pose;
00247     text_pose.position.x = cost_map_->cost_->size1()/2.0;
00248     text_pose.position.y = cost_map_->cost_->size1()/-20.0;
00249     text_pose.position.z = cost_map_->cost_->size1()/10.0;
00250 
00251     if (solved)
00252     {
00253       if (!simple_setup_->haveExactSolutionPath())
00254       {
00255         ROS_WARN_STREAM_NAMED("plan","APPROXIMATE solution found");
00256         if (use_visuals_)
00257           visual_tools_->publishText(text_pose, "APPROXIMATE solution found");
00258       }
00259       else
00260       {
00261         ROS_DEBUG_STREAM_NAMED("plan","Exact solution found");
00262         if (use_visuals_)
00263           visual_tools_->publishText(text_pose, "Exact solution found");
00264 
00265       }
00266 
00267       if (use_visuals_)
00268       {
00269         if (runs == 1)
00270         {
00271           // display all the aspects of the solution
00272           publishPlannerData(false);
00273         }
00274         else
00275         {
00276           // only display the paths
00277           publishPlannerData(false);
00278         }
00279       }
00280     }
00281     else
00282     {
00283       ROS_ERROR("No Solution Found");
00284       if (use_visuals_)
00285         visual_tools_->publishText(text_pose, "No Solution Found");
00286     }
00287 
00288     return solved;
00289   }
00290 
00291   void chooseStartGoal(ob::ScopedState<>& start, ob::ScopedState<>& goal)
00292   {
00293     if( true ) // choose completely random state
00294     {
00295       findValidState(start);
00296       findValidState(goal);
00297     }
00298     else if (false) // Manually set the start location
00299     {
00300       // Plan from scrach location
00301       start[0] = 5;  start[1] = 5;
00302       goal[0] = 5;  goal[1] = 45;
00303       // Recall location
00304       start[0] = 45;  start[1] = 5;
00305       goal[0] = 45;  goal[1] = 45;
00306     }
00307     else // Randomly sample around two states
00308     {
00309       ROS_INFO_STREAM_NAMED("temp","Sampling start and goal around two center points");
00310 
00311       ob::ScopedState<> start_area(space_);
00312       start_area[0] = 100;
00313       start_area[1] = 80;
00314 
00315       ob::ScopedState<> goal_area(space_);
00316       goal_area[0] = 330;
00317       goal_area[1] = 350;
00318 
00319       // Check these hard coded values against varying image sizes
00320       if (!space_->satisfiesBounds(start_area.get()) || !space_->satisfiesBounds(goal_area.get()))
00321       {
00322         ROS_ERROR_STREAM_NAMED("chooseStartGoal:","State does not satisfy bounds");
00323         exit(-1);
00324         return;
00325       }
00326 
00327       // Choose the distance to sample around
00328       double maxExtent = si_->getMaximumExtent();
00329       double distance = maxExtent * 0.1;
00330       ROS_INFO_STREAM_NAMED("temp","Distance is " << distance << " from max extent " << maxExtent);
00331 
00332       // Publish the same points
00333       if (true)
00334       {
00335         findValidState(start.get(), start_area.get(), distance);
00336         findValidState(goal.get(), goal_area.get(), distance);
00337       }
00338       else
00339       {
00340         //                start[0] = 277.33;  start[1] = 168.491;
00341         //                 goal[0] = 843.296;  goal[1] = 854.184;
00342       }
00343 
00344       // Show the sample regions
00345       if (use_visuals_ && false)
00346       {
00347         visual_tools_->publishSampleRegion(start_area, distance);
00348         visual_tools_->publishSampleRegion(goal_area, distance);
00349       }
00350     }
00351 
00352     // Print the start and goal
00353     //ROS_DEBUG_STREAM_NAMED("chooseStartGoal","Start: " << start);
00354     //ROS_DEBUG_STREAM_NAMED("chooseStartGoal","Goal: " << goal);
00355   }
00356 
00357   void findValidState(ob::ScopedState<>& state)
00358   {
00359     std::size_t rounds = 0;
00360     while (rounds < 100)
00361     {
00362       state.random();
00363 
00364       // Check if the sampled points are valid
00365       if( si_->isValid(state.get()) )
00366       {
00367         return;
00368       }
00369       ++rounds;
00370     }
00371     ROS_ERROR_STREAM_NAMED("findValidState","Unable to find valid start/goal state after " << rounds << " rounds");
00372   }
00373 
00374   void findValidState(ob::State *state, const ob::State *near, const double distance)
00375   {
00376     // Create sampler
00377     ob::StateSamplerPtr sampler = si_->allocStateSampler();
00378 
00379     while (true)
00380     {
00381       sampler->sampleUniformNear(state, near, distance); // samples (near + distance, near - distance)
00382 
00383       // Check if the sampled points are valid
00384       if( si_->isValid(state) )
00385       {
00386         return;
00387       }
00388       else
00389         ROS_INFO_STREAM_NAMED("temp","Searching for valid start/goal state");
00390     }
00391   }
00392 
00397   void publishPlannerData(bool just_path)
00398   {
00399     // Final Solution  ----------------------------------------------------------------
00400 
00401     if (true)
00402     {
00403       // Show basic solution
00404       //visual_tools_->publishPath( simple_setup_->getSolutionPath(), RED);
00405 
00406       // Simplify solution
00407       //simple_setup_->simplifySolution();
00408 
00409       // Interpolate solution
00410       simple_setup_->getSolutionPath().interpolate();
00411 
00412       // Show path
00413       visual_tools_->publishPath( simple_setup_->getSolutionPath(), rviz_visual_tools::GREEN, 1.0, "final_solution");
00414     }
00415 
00416     // Print the states to screen -----------------------------------------------------
00417     if (false)
00418     {
00419       ROS_DEBUG_STREAM_NAMED("temp","showing path");
00420       simple_setup_->getSolutionPath().print(std::cout);
00421     }
00422 
00423     // Get information about the exploration data structure the motion planner used in planning
00424     const ob::PlannerDataPtr planner_data( new ob::PlannerData( si_ ) );
00425     simple_setup_->getPlannerData( *planner_data );
00426 
00427     // Optionally display the search tree/graph or the samples
00428     if (!just_path)
00429     {
00430       // Visualize the explored space
00431       visual_tools_->publishGraph(planner_data, rviz_visual_tools::ORANGE, 0.2, "tree");
00432 
00433       // Visualize the sample locations
00434       //visual_tools_->publishSamples(planner_data);
00435     }
00436 
00437   }
00438 
00440   og::SimpleSetupPtr getSimpleSetup()
00441   {
00442     return simple_setup_;
00443   }
00444 
00445 
00446 }; // end of class
00447 
00448 } // namespace
00449 
00450 // *********************************************************************************************************
00451 // Main
00452 // *********************************************************************************************************
00453 int main( int argc, char** argv )
00454 {
00455   ros::init(argc, argv, "ompl_visual_tools");
00456   ROS_INFO( "OMPL Visual Tools Demo ----------------------------------------- " );
00457 
00458   // Seed random
00459   srand ( time(NULL) );
00460 
00461   // Allow the action server to recieve and send ros messages
00462   ros::AsyncSpinner spinner(1);
00463   spinner.start();
00464 
00465   // Default argument values
00466   bool verbose = false;
00467   bool use_visuals = true;
00468   std::string image_path;
00469   int runs = 1;
00470 
00471   if (argc > 1)
00472   {
00473     for (std::size_t i = 0; i < argc; ++i)
00474     {
00475       // Help mode
00476       if (strcmp(argv[i], "--help") == 0 || strcmp(argv[i], "-h") == 0)
00477       {
00478         ROS_INFO_STREAM_NAMED("main","Usage: ompl_rviz_demos"
00479                               << " --verbose --noVisuals --image [image_file] --runs [num plans] "
00480                               << " -h --help");
00481         return 0;
00482       }
00483 
00484       // Check for verbose flag
00485       if (strcmp(argv[i], "--verbose") == 0)
00486       {
00487         ROS_INFO_STREAM_NAMED("main","Running in VERBOSE mode (slower)");
00488         verbose = true;
00489       }
00490 
00491       // Check if we should publish markers
00492       if (strcmp(argv[i], "--noVisuals") == 0)
00493       {
00494         ROS_INFO_STREAM_NAMED("main","NOT displaying graphics");
00495         use_visuals = false;
00496       }
00497 
00498       // Check if user has passed in an image to read
00499       if (strcmp(argv[i], "--image") == 0)
00500       {
00501         i++; // get next arg
00502         image_path = argv[i];
00503       }
00504 
00505       // Check if user has passed in the number of runs to perform
00506       if (strcmp(argv[i], "--runs") == 0)
00507       {
00508         i++; // get next arg
00509         runs = atoi( argv[i] );
00510       }
00511     }
00512   }
00513 
00514   // Provide image if necessary
00515   if (image_path.empty()) // use default image
00516   {
00517     // Get image path based on package name
00518     image_path = ros::package::getPath("ompl_visual_tools");
00519     if( image_path.empty() )
00520     {
00521       ROS_ERROR( "Unable to get OMPL Visual Tools package path " );
00522       return false;
00523     }
00524 
00525     // Choose random image
00526     int rand_num = ompl_visual_tools::OmplVisualTools::dRand(0,2);
00527     switch( rand_num )
00528     {
00529       case 0:
00530         image_path.append( "/resources/wilbur_medium/wilbur_medium1.ppm" );
00531         break;
00532       case 1:
00533         image_path.append( "/resources/wilbur_medium/wilbur_medium2.ppm" );
00534         break;
00535       default:
00536         ROS_ERROR_STREAM_NAMED("main","Random has no case " << rand_num);
00537         break;
00538     }
00539   }
00540 
00541   // Create the planner
00542   ompl_visual_tools::RRTDemo demo(verbose, use_visuals);
00543 
00544   // Clear Rviz
00545   demo.deleteAllMakers();
00546 
00547   // Load an image
00548   ROS_INFO_STREAM_NAMED("main","Loading image " << image_path);
00549   demo.loadCostMapImage( image_path, 0.4 );
00550   demo.publishCostMapImage();
00551 
00552   // Run the demo the desired number of times
00553   for (std::size_t i = 0; i < runs; ++i)
00554   {
00555     // Check if user wants to shutdown
00556     if (!ros::ok())
00557     {
00558       ROS_WARN_STREAM_NAMED("plan","Terminating early");
00559       break;
00560     }
00561     ROS_INFO_STREAM_NAMED("plan","Planning " << i+1 << " out of " << runs << " ------------------------------------");
00562 
00563     // Refresh visuals
00564     if (use_visuals && i > 0)
00565     {
00566       demo.publishCostMapImage();
00567       ros::spinOnce();
00568     }
00569 
00570     // Run the planner
00571     demo.plan( i, runs );
00572 
00573     // Create a pause if we are doing more runs and this is not the last run
00574     if (runs > 1 && i < runs - 1 && use_visuals)
00575     {
00576       // Let publisher publish
00577       ros::spinOnce();
00578       ros::Duration(5.0).sleep();
00579     }
00580 
00581     // Clear markers if this is not our last run
00582     if (i < runs - 1 && use_visuals)
00583       demo.deleteAllMakers();
00584   }
00585 
00586   // Wait to let anything still being published finish
00587   ros::Duration(0.1).sleep();
00588 
00589   ROS_INFO_STREAM("Shutting down.");
00590 
00591   return 0;
00592 }


ompl_visual_tools
Author(s): Dave Coleman
autogenerated on Thu Feb 11 2016 23:58:14