main.cc

来自「机器人人3D仿真工具,可以加入到Simbad仿真环境下应用。」· CC 代码 · 共 416 行

/*
 *  Gazebo - Outdoor Multi-Robot Simulator
 *  Copyright (C) 2003  
 *     Nate Koenig & Andrew Howard
 *
 *  This program is free software; you can redistribute it and/or modify
 *  it under the terms of the GNU General Public License as published by
 *  the Free Software Foundation; either version 2 of the License, or
 *  (at your option) any later version.
 *
 *  This program is distributed in the hope that it will be useful,
 *  but WITHOUT ANY WARRANTY; without even the implied warranty of
 *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 *  GNU General Public License for more details.
 *
 *  You should have received a copy of the GNU General Public License
 *  along with this program; if not, write to the Free Software
 *  Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
 *
 */
/* Desc: Builder main 
 * Author: Nate Keonig
 * Date: 02 Sept 2003
 * CVS: $Id: main.cc,v 1.14 2005/05/06 17:18:55 natepak Exp $
 */

/**

@page gzbuilder Terrain Builder Utility

The terrain builder processes an elevation data file and outputs a
binary data file loadable by the @ref Terrain model. The input file can be
of any type loadable by GDAL, see
http://www.gdal.org/formats_list.html for a complete list.

Basic usage is as follows:

@verbatim
$ gzbuilder [options] -i <inputfile> -o <outputfile>
@endverbatim

where [options] is one or more of the following:

\param -i &lt;filename&gt;   Input terrain file.

\param -o &lt;filename&gt;   Output Gazebo terrain file.

\param -e &lt;double&gt; Acceptable error bound on terrain
approximation (meters).  Set this to zero for no approximation.

\param -h &lt;double&gt;     Horizontal scaling factor (m).

\param -v &lt;double&gt;     Vertical scaling factor (m).

\param -n              Normalize Z-values before scaling.

\param -x &lt;double&gt;     X offset (m).

\param -y &lt;double&gt;     Y offset (m).

\param -z &lt;double&gt;     Z offset (m).

\param -u &lt;int&gt;        UTM zone. Default is 11

\param -s &lt;double&gt;     X size of texture (meters)

\param -t &lt;double&gt;     Y size of texture (meters)


@par Example: Building a maze

Maze-like environments can be built by providing gzbuilder with a
simple monochrome image.  The figure, for example, shows an input
image and resultant terrain as visualized by the server.

@image html maze.gif "Input image"
@image html maze_gzb.gif "Output terrain"

The terrain file was generated using the following command:

@verbatim
$ gzbuilder -i maze.gif -o maze.gzb -n -v 1.8 -h 0.1 -e 0.1
@endverbatim

where the options are:

- @c -n : Normalize the input image values in the range 0 (black) to 1
  (white).

- @c -v 1.8 : Scale heights such that white pixels correspond to an
  elevation of 1.8m.

- @c -h 0.1 : Scale image such that each pixel corresponds to a
  horizontal distance of 0.1m (i.e., a 10cm grid spacing).

- @c -e 0.1 : Generate an approximated terrain whose maximum elevation
  error is no more than 0.1m.

Note changing the error parameter can radically alter the output
terrain: setting this value too low will result in a slow simulation (many
vertices); setting this value too high will result in a "melted"
maze.

By default, the terrain is located such that the bottom-left corner of
the image maps to (0, 0) in simulator coordinates.  This can
be offset using the @c xyz tag in the @ref Terrain model.



Note also that the terrain can be arbitrarily translated and rotated
by the terrain model; by
  

@par Example: Building and using a geo-registered terrain

Digital Elevation Maps (DEMs) of real environments can be obtained
from various sources on the internet (some are free, some are
less-than-free).  These maps are generally geo-registered; i.e., they
have built-in latitude, logitude and altitude offsets.  The gzbuilder
utility will convert these offsets into standard UTM
coordinates (using the @c -u UTM zone option) and store this offset the terrain
file.  One can also take a simple image (with no lat/lon/alt data),
and apply an arbitrary UTM offset using the @c -x @c -y @c -z options.

When using geo-registered terrains, it is necessary to define a UTM
offset in the @ref models_global "global parameters" section of the
world file.  This offset governs the transformation between simulation
coordinates and UTM coordinates; i.e., the point (0, 0, 0) in the
simulation frame is mapped to point (utmOffset.x, utmOffset.y,
utmOffset.z) in the UTM frame.  The UTM coordinates printed by the
terrain builder may be used for this purpose.




@par Output File Format

The binary file format is composed of a header followed by
<tt>1..VertexCount</tt> of vertex datum followed by
<tt>1..IndexCount</tt> of index datum followed by
<tt>1..ODEIndexCount</tt> of ODE index datum.

All data is in network byte order, and each value is 4 bytes in
length.

A multiplication factor must be divided back out upon reading a value.

<table border=1 cellspacing=0 cellpadding=4>
  <tr>
    <th colspan=2>Header</th>
  </tr>
  <tr>
    <th>Value Description</th>
    <th>Original Data Type</th>
  </tr>
  <tr>
    <td>version number * 1e3</td>
    <td>float</td>
  </tr>
  <tr>
    <td>X-offset Whole</td> 
    <td>int</td>
  </tr>
  <tr>
    <td>X-offset Fractional * 1e6</td> 
    <td>float</td>
  </tr>
  <tr>
    <td>Y-offset Whole</td> 
    <td>int</td>
  </tr>
  <tr>
    <td>Y-offset Fractional * 1e6</td> 
    <td>float</td>
  </tr>
  <tr>
    <td>Z-offset Whole</td> 
    <td>int</td>
  </tr>
  <tr>
    <td>Z-offset Fractional * 1e6</td> 
    <td>float</td>
  </tr>
  <tr>
    <td>Vertex Count</td>
    <td>int</td>
  </tr>
  <tr>
    <td>Index Count</td>
    <td>int</td>
  </tr>
   <tr>
    <td>ODE Index Count</td>
    <td>int</td>
  </tr>
</table>

For Each Vertex:
<br>
<table border=1 cellspacing=0 cellpadding=4>
  <tr>
    <th colspan=2>Vertex Datum</th>
  </tr>
  <tr>
    <th>Value Description</th>
    <th>Original Data Type</th>
  </tr>
  <tr>
    <td>S Texture Coord * 1e3</td>
    <td>float</td>
  </tr>
  <tr>
    <td>T Texture Coord * 1e3</td>
    <td>float</td>
  </tr>
  <tr>
    <td>Red Color * 1e3</td>
    <td>float</td>
  </tr>
  <tr>
    <td>Green Color * 1e3</td>
    <td>float</td>
  </tr>
  <tr>
    <td>Blue Color * 1e3</td>
    <td>float</td>
  </tr>
  <tr>
    <td>Alpha Color * 1e3</td>
    <td>float</td>
  </tr>
  <tr>
    <td>X Normal * 1e3</td>
    <td>float</td>
  </tr>
  <tr>
    <td>Y Normal * 1e3</td>
    <td>float</td>
  </tr>
  <tr>
    <td>Z Normal * 1e3</td>
    <td>float</td>
  </tr>
  <tr>
    <td>X Position * 1e3</td>
    <td>float</td>
  </tr>
  <tr>
    <td>Y Position * 1e3</td>
    <td>float</td>
  </tr>
  <tr>
    <td>Z Position * 1e3</td>
    <td>float</td>
  </tr>
</table>


For Each Index:
<br>
<table border=1 cellspacing=0 cellpadding=4>
  <tr>
    <th colspan=2>Index Datum</th>
  </tr>
  <tr>
    <th>Value Description</th>
    <th>Original Data Type</th>
  </tr>
  <tr>
    <td>Index Value</td>
    <td>int</td>
  </tr>
</table>

For Each ODE Index:
<table border=1 cellspacing=0 cellpadding=4>
  <tr>
    <th colspan=2>Index Datum</th>
  </tr>
  <tr>
    <th>Value Description</th>
    <th>Original Data Type</th>
  </tr>
  <tr>
    <td>ODE Index Value</td>
    <td>int</td>
  </tr>
</table>

*/
/** @} */


#include <string.h>
#include <stdlib.h>
#include <stdio.h>
#include <getopt.h>

#include "Terrain.hh"

char *inputFilename = NULL;
char *outputFilename = NULL;

Terrain terrain;

//////////////////////////////////////////////////////////////////////////////
// Parse the arguments
void ParseArgs( int argc, char * const argv[] )
{
  int idx;
  const char *optstring = "i:o:e:h:v:x:y:z:s:t:u:n";

  while ((idx=getopt(argc, argv, optstring)) != -1)
  {
    switch (idx)
    {
      case 'i':
        inputFilename = new char[strlen(optarg)+1];
        strcpy(inputFilename, optarg);
        break;
      case 'o':
        outputFilename = new char[strlen(optarg)+1];
        strcpy(outputFilename, optarg);
        break;
      case 'e':
        terrain.SetErrorBound(atof(optarg));
        break;
      case 'h':
        terrain.SetHorzScale(atof(optarg));
        break;
      case 'v':
        terrain.SetVertScale(atof(optarg));
        break;
      case 'x':
        terrain.SetXOffset(atof(optarg));
        break;
      case 'y':
        terrain.SetYOffset(atof(optarg));
        break;
      case 'z':
        terrain.SetZOffset(atof(optarg));
        break;
      case 'n':
        terrain.SetNormalizeZ(true);
        break;
      case 'u':
        terrain.SetUTMZone(atoi(optarg));
        break;
      case 's':
        terrain.SetSTextureSize(atof(optarg));
        break;
      case 't':
        terrain.SetTTextureSize(atof(optarg));
        break;
      case '?':
        printf("option [0%o] unkown\n", idx);
        break;
      default:
        printf("getopt returned character code 0%o\n",idx);
        break;
    }
  }
}

//////////////////////////////////////////////////////////////////////////////
// Print a help message
void PrintHelp()
{
  printf("\t -i <filename>   Input terrain file\n");
  printf("\t -o <filename>   Output Gazebo terrain file\n");
  printf("\t -e <double>     Error bound (meters)\n");
  printf("\t -h <double>     Horizontal scaling factor\n");
  printf("\t -v <double>     Vertical scaling factor\n");
  printf("\t -n              Normalize Z-values before scaling\n");
  printf("\t -x <double>     X offset\n");
  printf("\t -y <double>     Y offset\n");
  printf("\t -z <double>     Z offset\n");
  printf("\t -u <int>        UTM zone. Default is 11\n");
  printf("\t -s <double>     Texture x size (meters)\n");
  printf("\t -t <double>     Texture y size (meters)\n");
}

//////////////////////////////////////////////////////////////////////////////
// The main function
int main( int argc, char *argv[] )
{
  ParseArgs(argc, argv);

  // Need an input file
  if (inputFilename == NULL)
  {
    fprintf(stderr, "Error: No input file\n");
    PrintHelp();
    return -1;
  }
  
  if (outputFilename == NULL)
  {
    fprintf(stderr, "Error: No output file\n");
    PrintHelp();
    return -1;
  }

  // Load the terrain
  if (terrain.Load(inputFilename))
    return 0;

  // Print some useful information
  terrain.PrintInfo();

  // Output the terrain in a Gazebo friendly format
  terrain.Write(outputFilename);

  return 0;
}