cost_map_2d_optimization_objective.h
Go to the documentation of this file.
00001 /*********************************************************************
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Copyright (c) 2013, University of Colorado, Boulder
00005  *  All rights reserved.
00006  *
00007  *  Redistribution and use in source and binary forms, with or without
00008  *  modification, are permitted provided that the following conditions
00009  *  are met:
00010  *
00011  *   * Redistributions of source code must retain the above copyright
00012  *     notice, this list of conditions and the following disclaimer.
00013  *   * Redistributions in binary form must reproduce the above
00014  *     copyright notice, this list of conditions and the following
00015  *     disclaimer in the documentation and/or other materials provided
00016  *     with the distribution.
00017  *   * Neither the name of the Univ of CO, Boulder nor the names of its
00018  *     contributors may be used to endorse or promote products derived
00019  *     from this software without specific prior written permission.
00020  *
00021  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032  *  POSSIBILITY OF SUCH DAMAGE.
00033  *********************************************************************/
00034 
00035 /* Author: Dave Coleman
00036  * Desc:   Optimization objective that simply reads a value from a 2D cost map
00037  */
00038 
00039 
00040 #ifndef OMPL_VISUAL_TOOLS__COST_MAP_OPTIMIZATION_OBJECTIVE_
00041 #define OMPL_VISUAL_TOOLS__COST_MAP_OPTIMIZATION_OBJECTIVE_
00042 
00043 // OMPL
00044 #include <ompl/base/OptimizationObjective.h>
00045 #include <ompl/base/SpaceInformation.h>
00046 #include <ompl/base/Cost.h>
00047 #include <ompl/base/spaces/RealVectorStateSpace.h>
00048 
00049 // Boost
00050 #include <boost/numeric/ublas/matrix.hpp>
00051 #include <boost/numeric/ublas/io.hpp>
00052 
00053 // For reading image files
00054 #include <ompl_visual_tools/utilities/ppm.h>
00055 
00056 namespace ompl_visual_tools
00057 {
00058 typedef boost::numeric::ublas::matrix<int> intMatrix;
00059 typedef boost::shared_ptr<intMatrix> intMatrixPtr;
00060 }
00061 
00062 namespace ob = ompl::base;
00063 namespace og = ompl::geometric;
00064 
00065 namespace ompl
00066 {
00067 namespace base
00068 {
00070 class CostMap2DOptimizationObjective : public OptimizationObjective
00071 {
00072 public:
00074     CostMap2DOptimizationObjective(const SpaceInformationPtr &si, double pathLengthWeight = 0.0001)
00075         : OptimizationObjective(si),
00076           max_cost_threshold_percent_(0.4),
00077           image_(NULL),
00078           pathLengthWeight_(pathLengthWeight)
00079     {
00080         description_ = "Cost Map";
00081 
00082         cost_.reset(new ompl_visual_tools::intMatrix());
00083     };
00084 
00086     ~CostMap2DOptimizationObjective()
00087     {
00088         delete image_;
00089     };
00090 
00091     double getPathLengthWeight() const
00092     {
00093         return pathLengthWeight_;
00094     }
00095 
00097     virtual ompl::base::Cost motionCost(const State *s1, const State *s2) const
00098     {
00099         // Only accrue positive changes in cost
00100       double positiveCostAccrued = std::max(stateCost(s2).value() - stateCost(s1).value(), 0.0);
00101         return Cost(positiveCostAccrued + pathLengthWeight_*si_->distance(s1,s2));
00102     };
00103 
00104     ompl::base::Cost stateCost(const State *state) const
00105     {
00106         const double *coords = state->as<ob::RealVectorStateSpace::StateType>()->values;
00107 
00108         // Return the cost from the matrix at the current dimensions
00109         double cost = (*cost_)( natRound(coords[1]), natRound(coords[0]) );
00110 
00111         return Cost(cost);
00112     }
00113 
00115     void setCostMatrix(ompl_visual_tools::intMatrixPtr cost)
00116     {
00117         cost_ = cost;
00118     };
00119 
00125     int natRound(double x) const
00126     {
00127         return static_cast<int>(floor(x + 0.5f));
00128     };
00129 
00130     void loadImage( std::string image_path )
00131     {
00132         // Load cost map from image file
00133         image_ = ompl_visual_tools::readPPM( image_path.c_str() );
00134 
00135         // Error check
00136         if( !image_ )
00137         {
00138             ROS_ERROR( "No image data loaded " );
00139             return;
00140         }
00141 
00142         // Disallow non-square
00143         if( image_->x != image_->y )
00144         {
00145             ROS_ERROR( "Does not currently support non-square images because of some weird bug. Feel free to fork and fix!" );
00146             return;
00147         }
00148 
00149         ROS_INFO_STREAM( "Map Height: " << image_->y << " Map Width: " << image_->x );
00150 
00151         // Create an array of ints that represent the cost of every pixel
00152         cost_->resize( image_->x, image_->y );
00153 
00154         // Generate the cost map
00155         createCostMap();
00156     };
00157 
00161     void createCostMap()
00162     {
00163         // gets the min and max values of the cost map
00164         getMinMaxPixels();
00165 
00166         // This factor is the author's visual preference for scaling a cost map in Rviz
00167         const double artistic_scale = 6.0; // smaller is taller
00168 
00169         const double pixel_diff = max_pixel_ - min_pixel_;
00170 
00171         // This scale adapts that factor depending on the cost map min max
00172         const double scale = pixel_diff / ( image_->x / artistic_scale ); //image->x is width
00173 
00174         // Dynamically calculate the obstacle threshold
00175         max_cost_threshold_ = (max_pixel_ - ( max_cost_threshold_percent_ * pixel_diff )) / scale;
00176 
00177         // Preprocess the pixel data for cost and give it a nice colored tint
00178         for( size_t i = 0; i < image_->getSize(); ++i )
00179         {
00180             // Calculate cost
00181             cost_->data()[i]  = ( image_->data[ i ].red - min_pixel_ ) / scale;
00182 
00183             // Prevent cost from being zero
00184             if( !cost_->data()[i] )
00185                 cost_->data()[i] = 1;
00186 
00187             // Color different if it is an obstacle
00188             if( cost_->data()[i] >= max_cost_threshold_ || cost_->data()[i] <= 1)
00189             {
00190                 //std::cout << "cost is " <<  cost_->data()[i] << " threshold is " <<  max_cost_threshold_ << std::endl;
00191 
00192                 image_->data[ i ].red = 255; //image_->data[ i ].red;
00193                 image_->data[ i ].green = image_->data[ i ].green;
00194                 image_->data[ i ].blue = image_->data[ i ].blue;
00195             }
00196 
00197         }
00198 
00199     }
00200 
00204     void getMinMaxPixels()
00205     {
00206         // Find the min and max cost from the image
00207         min_pixel_ = image_->data[ 0 ].red;
00208         max_pixel_ = image_->data[ 0 ].red;
00209 
00210         for( size_t i = 0; i < image_->getSize(); ++i )
00211         {
00212             // Max
00213             if( image_->data[ i ].red > max_pixel_ )
00214                 max_pixel_ = image_->data[ i ].red;
00215             // Min
00216             else if( image_->data[ i ].red < min_pixel_ )
00217                 min_pixel_ = image_->data[ i ].red;
00218         }
00219     }
00220 
00221     // The RGB image data
00222     ompl_visual_tools::PPMImage *image_;
00223 
00224     // The cost for each x,y - which is derived from the RGB data
00225     ompl_visual_tools::intMatrixPtr cost_;
00226 
00227     // The cost at which it becomes an obstacle
00228     double max_cost_threshold_;
00229 
00230     // The percentage of the top min/max cost value that is considered an obstacle, e.g. 0.1 is top 10% of peaks
00231     double max_cost_threshold_percent_;
00232 
00233 protected:
00234 
00236     double pathLengthWeight_;
00237 
00238     // Remember the min and max cost from the image
00239     int max_pixel_;
00240     int min_pixel_;
00241 
00242 };
00243 
00244 typedef boost::shared_ptr< CostMap2DOptimizationObjective > CostMap2DOptimizationObjectivePtr;
00245 }
00246 }
00247 
00248 #endif
00249 
00250 


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