flirtlib_ros: flirtlib_ros: generate_simulated

00001 /*
00002  * Copyright (c) 2008, Willow Garage, Inc.
00003  * All rights reserved.
00004  *
00005  * Redistribution and use in source and binary forms, with or without
00006  * modification, are permitted provided that the following conditions are met:
00007  *
00008  *     * Redistributions of source code must retain the above copyright
00009  *       notice, this list of conditions and the following disclaimer.
00010  *     * Redistributions in binary form must reproduce the above copyright
00011  *       notice, this list of conditions and the following disclaimer in the
00012  *       documentation and/or other materials provided with the distribution.
00013  *     * Neither the name of the Willow Garage, Inc. nor the names of its
00014  *       contributors may be used to endorse or promote products derived from
00015  *       this software without specific prior written permission.
00016  *
00017  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00018  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00019  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
00020  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
00021  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
00022  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
00023  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
00024  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
00025  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
00026  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00027  * POSSIBILITY OF SUCH DAMAGE.
00028  *
00029  */
00030 
00039 #include <occupancy_grid_utils/file.h>
00040 #include <occupancy_grid_utils/ray_tracer.h>
00041 #include <tf/transform_datatypes.h>
00042 #include <mongo_ros/message_collection.h>
00043 #include <mongo_ros/mongo_ros.h>
00044 #include <flirtlib_ros/flirtlib.h>
00045 #include <flirtlib_ros/conversions.h>
00046 #include <boost/program_options.hpp>
00047 
00048 namespace sm=sensor_msgs;
00049 namespace gm=geometry_msgs;
00050 namespace fr=flirtlib_ros;
00051 namespace mr=mongo_ros;
00052 namespace nm=nav_msgs;
00053 namespace gu=occupancy_grid_utils;
00054 namespace po=boost::program_options;
00055 
00056 using std::cerr;
00057 using std::endl;
00058 using std::string;
00059 
00060 const double PI = 3.14159265;
00061 
00062 gm::Pose makePose (const double x, const double y, const double theta)
00063 {
00064   gm::Pose p;
00065   p.position.x = x;
00066   p.position.y = y;
00067   p.orientation = tf::createQuaternionMsgFromYaw(theta);
00068   return p;
00069 }
00070 
00071 
00072 int main (int argc, char** argv)
00073 {
00074   ros::init(argc, argv, "generate_simulated_scans");
00075 
00076   // Parse args
00077   double resolution;
00078   gm::Pose origin;
00079   double origin_theta;
00080   double disc;
00081   string db_name;
00082   int num_angles;
00083   string filename;
00084   po::options_description desc("Allowed options");
00085   desc.add_options()
00086     ("help,h", "Print help message")
00087     ("resolution,r", po::value<double>(&resolution),
00088      "Map resolution (required)")
00089     ("origin_x,x", po::value<double>(&origin.position.x)->default_value(0),
00090      "Origin x")
00091     ("origin_y,y", po::value<double>(&origin.position.y)->default_value(0),
00092      "Origin y")
00093     ("origin_theta,t", po::value<double>(&origin_theta)->default_value(0),
00094      "Origin yaw")
00095     ("discretization,z", po::value<double>(&disc)->default_value(1.0),
00096      "Scan discretization (default 1.0)")
00097     ("database_name,d", po::value<string>(&db_name),
00098      "Name of database (required)")
00099     ("filename,f", po::value<string>(&filename), "Map file (required)")
00100     ("num_angles,a", po::value<int>(&num_angles)->default_value(4),
00101      "Number of angles considered at each position")
00102     ;
00103   po::variables_map vm;
00104   po::store(po::parse_command_line(argc, argv, desc), vm);
00105   po::notify(vm);
00106 
00107   if (vm.count("help") || !vm.count("resolution") || !vm.count("filename") ||
00108       !vm.count("database_name"))
00109   {
00110     cerr << desc << endl;
00111     return 1;
00112   }
00113   
00114   // Load occupancy grid from file
00115   origin.orientation = tf::createQuaternionMsgFromYaw(origin_theta);
00116   nm::OccupancyGrid::Ptr grid = gu::loadGrid(filename, resolution, origin);
00117   cerr << "Loaded a " << grid->info.height << "x" << grid->info.width <<
00118     " map at " << grid->info.resolution << " m/cell with origin " <<
00119     origin;
00120   gm::Polygon grid_poly = gu::gridPolygon(grid->info);
00121   double x_min=1e9, y_min=1e9, x_max=-1e9, y_max=-1e9;
00122   ROS_ASSERT (grid_poly.points.size()==4);
00123   BOOST_FOREACH (const gm::Point32& corner, grid_poly.points) 
00124   {
00125     if (corner.x < x_min)
00126       x_min = corner.x;
00127     if (corner.x > x_max)
00128       x_max = corner.x;
00129     if (corner.y < y_min)
00130       y_min = corner.y;
00131     if (corner.y > y_max)
00132       y_max = corner.y;
00133   }
00134   ROS_ASSERT (x_min<1e9 && y_min<1e9 && x_max>-1e9 && y_max>-1e9);
00135   ROS_ASSERT_MSG (origin_theta==0, "Can't handle rotated maps");
00136   
00137   // Set up scan info
00138   // \todo PR2 specific values: allow changing
00139   sm::LaserScan scan_info;
00140   scan_info.angle_min = -2.26892805099;
00141   scan_info.angle_max = 2.26456475258;
00142   scan_info.angle_increment = 0.00436332309619;
00143   scan_info.range_max = 60.0;
00144   
00145   // Set up db connection and remove any existing scans
00146   mr::dropDatabase(db_name, "localhost", 27017);
00147   mr::MessageCollection<fr::RefScanRos> scans(db_name, "scans", "localhost");
00148 
00149   // Set up feature extractor
00150   fr::FlirtlibFeatures features;
00151 
00152   // Note loop is using the above assumption that origin_theta = 0
00153   for (double x=x_min+disc; x<x_max; x+=disc)
00154   {
00155     cerr << "x = " << x << endl;
00156     for (double y=y_min+disc; y<y_max; y+=disc)
00157     {
00158       gm::Point pt;
00159       pt.x = x;
00160       pt.y = y;
00161       if (grid->data[gu::pointIndex(grid->info, pt)]!=gu::UNOCCUPIED)
00162         continue;
00163       cerr << "  y = " << y << endl;
00164       for (double theta=0; theta<2*PI; theta += 2*PI/num_angles)
00165       {
00166         const gm::Pose& p = makePose(x, y, theta);
00167         sm::LaserScan::Ptr scan = gu::simulateRangeScan(*grid, p, scan_info);
00168         fr::InterestPointVec pts = features.extractFeatures(scan);
00169         sm::LaserScan::Ptr stripped_scan(new sm::LaserScan(*scan));
00170         stripped_scan->ranges.clear();
00171         stripped_scan->intensities.clear();
00172         fr::RefScan ref_scan(stripped_scan, p, pts);
00173         scans.insert(toRos(ref_scan),
00174                       mr::Metadata("x", x, "y", y, "theta", theta));
00175       }
00176     }
00177   }
00178 }
generate_simulated_scans.cpp
