road.cc

机器人人3D仿真工具,可以加入到Simbad仿真环境下应用。

/*
 *  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: Model for a road
 * Author: Andrew Howard
 * Date: 23 Oct 2004
 * CVS: $Id: Road.cc,v 1.17 2005/05/06 17:09:30 natepak Exp $
 */

/// @addtogroup models
/// @{
/** @defgroup Road Road

Generates a road surface using a Catmull-Rom spline based on points from a descriptor file. 

@par libgazebo interfaces

This model has no libgazebo interfaces.

@par Player drivers

No Player drivers are available.

@par Attributes

Road models can be instantiated using the <tt><model:Road></tt> tag.
The following attributes are supported.

@htmlinclude default_attr_include.html

- hardness (float tuple)
  - Set spring-damper parameters for contact joints
  - Default: dInfinity 0

- friction (float tuple)
  - Set friction coefficients
  - Default: 1 1

- color (float tuple)
  - RGB color.
  - Default: 0 0 0

- textureFile (string, filename)
  - Texture image file.
  - Default: empty

- textureSize (float tuple, meters)
  - Size of the texture image when projected onto the ground plane (m).
  - Default: 2.0 2.0

- roadFile (string, filename)
  - Road data filename.
  - Default: empty

- roadSpacing (float, meters)
  - Nominal spacing for points in the roadFile.
  - Default: 5 1

@par Bodies

The following bodies are created by this model.

@htmlinclude default_body_include.html

@par Example

@verbatim
<model:Road>
  <xyz>0 0 0</xyz>
</model:Road>
@endverbatim

@par Authors

Andrew Howard

*/
/// @}

#if HAVE_CONFIG_H
#include <config.h>
#endif

#include <assert.h>
#include <errno.h>
#include <string.h>
#include <stdio.h>

#include "gazebo.h"
#include "Error.hh"
#include "World.hh"
#include "WorldFile.hh"
#include "ModelFactory.hh"
#include "Body.hh"
#include "RoadGeom.hh"

#include "Road.hh"


//////////////////////////////////////////////////////////////////////////////
// Register this model
GZ_REGISTER_STATIC("Road", Road);


//////////////////////////////////////////////////////////////////////////////
// Constructor
Road::Road( World *world )
    : Model( world )
{
  this->body = NULL;
  this->roadGeom = NULL;
  return;
}


//////////////////////////////////////////////////////////////////////////////
// Destructor
Road::~Road()
{
  if (this->body)
    delete this->body;
  if (this->roadGeom)
    delete this->roadGeom;
  
  this->body = NULL;
  this->roadGeom = NULL;
  return;
}


//////////////////////////////////////////////////////////////////////////////
// Load the model
int Road::Load(WorldFile *file, WorldFileNode *node)
{
  // Create the ODE objects
  if (this->LoadODE(file, node) != 0)
    return -1;

  // Load the road description
  if (this->LoadRoad(file, node) != 0)
    return -1;

  return 0;
}


//////////////////////////////////////////////////////////////////////////////
// Load ODE objects
int Road::LoadODE(WorldFile *file, WorldFileNode *node)
{
  const char *textureFile;
  
  this->body = new Body(this->world, NULL, true);
  this->AddBody(this->body, true);
  
  this->roadGeom = new RoadGeom(this->body, this->modelSpaceId);
  
  // Surface hardness
  this->roadGeom->SetHardness(node->GetTupleDouble("surfaceHardness", 0, dInfinity),
                              node->GetTupleDouble("surfaceHardness", 1, 0));

  // Surface friction
  this->roadGeom->SetFriction(node->GetTupleDouble("surfaceFriction", 0, 1.0),
                              node->GetTupleDouble("surfaceFriction", 1, 1.0));

  // Color
  this->roadGeom->SetColor(node->GetColor("color", GzColor(0.7, 0.7, 0)));

  // Apply a texture
  textureFile = node->SearchFilename("textureFile", GAZEBO_TEXTURES_PATH, NULL);
  if (textureFile)
  {
    PRINT_MSG1(1, "loading texture file [%s]", textureFile);  
    if (this->roadGeom->SetTexture2DFile(textureFile) != 0)
    {
      PRINT_ERR("unable to load texture file");
      return -1;
    }
  }
  this->roadGeom->SetTexture2DSize(node->GetPosition("textureSize", GzVectorSet(2.0, 2.0, 0.0)));
  
  dGeomSetCategoryBits((dGeomID) this->modelSpaceId, GZ_FIXED_COLLIDE);
  dGeomSetCollideBits((dGeomID) this->modelSpaceId, ~GZ_FIXED_COLLIDE);

  return 0;
}


//////////////////////////////////////////////////////////////////////////////
// Load the road description
int Road::LoadRoad(WorldFile *file, WorldFileNode *node)
{
  const char *filename;
  FILE *rfile;
  char line[1024];
  char *tokens[1024];
  int lineCount, tokenCount;
  int i, wp, index, profileCount;
  double utme, utmn, alt;
  GzVector p;
  char *type;
  char text[128];
  double dist;
  
  // Get the road file
  filename = node->SearchFilename("roadFile", NULL, NULL);
  if (!filename)
  {
    PRINT_ERR("no road file specified");
    return -1;
  }
    
  // Nominal spacing for points in the file
  this->roadGeom->SetTrackSpacing(node->GetTupleLength("roadSpacing", 0, 5.0));
  this->roadGeom->SetProfileSpacing(node->GetTupleLength("roadSpacing", 1, 1.0));
    
  // Open file
  rfile = fopen(filename, "r");
  if (rfile == NULL)
  {
    PRINT_ERR2("unable to open [%s] : %s", filename, strerror(errno));
    return -1;
  }
  
  lineCount = 0;
  
  // Parse the file
  while (true)
  {      
    if (fgets(line, sizeof(line), rfile) == NULL)
      break;
    lineCount++;

    // Break line into tokens
    tokenCount = 0;
    tokens[tokenCount] = strtok(line, " ");
    while (tokens[tokenCount])
    {
      tokenCount++;
      assert(tokenCount < (int) (sizeof(tokens) / sizeof(tokens[0])));
      tokens[tokenCount] = strtok(NULL, " ");
    }

    if (tokenCount < 5)
    {
      PRINT_ERR1("syntax error in road file, line [%d]", lineCount);
      break;
    }

    // See what kind of line this is
    type = tokens[0];
    dist = atof(tokens[1]);
    wp = atoi(tokens[2]);
    utme = atof(tokens[3]);
    utmn = atof(tokens[4]);
    alt = atof(tokens[5]);

    // If this is a waypoint entry
    if (strcmp(type, "waypoint") == 0)
    {
      // Compute position in global cs
      p = GzVectorSet(utme, utmn, alt);
      p = GzVectorSub(p, this->world->utmOffset);           

      snprintf(text, sizeof(text), "WP %d %.3fkm", wp, dist / 1e3);
      
      this->roadGeom->AddSign(p, text);
    }
    
    // If this is a track entry
    else if (strcmp(type, "track") == 0)
    {
      // Compute track point in global cs
      p = GzVectorSet(utme, utmn, alt);
      p = GzVectorSub(p, this->world->utmOffset);           
      index = this->roadGeom->AddTrackPoint(p);

      // Check for errors
      if (tokenCount < 7)
      {
        PRINT_ERR1("syntax error in road file, line [%d]", lineCount);
        break;
      }

      // Parse profile values
      profileCount = atoi(tokens[6]);

      // Check for errors
      if (tokenCount < 6 + profileCount)
      {
        PRINT_ERR1("syntax error in road file, line [%d]", lineCount);
        break;
      }

      // Extract profile points
      for (i = 0; i < profileCount; i++)
      {
        alt = atof(tokens[7 + i]);
        this->roadGeom->AddProfilePoint(index, alt);
      }
    }        
  }

  // See if we got to the end
  if (!feof(rfile))
  {
    fclose(rfile);
    return -1;
  }
  
  fclose(rfile);
  return 0;
}


//////////////////////////////////////////////////////////////////////////////
// Initialize the model
int Road::Init(WorldFile *file, WorldFileNode *node)
{
  //int i;
  //Model *model;

  /* TODO
  this->trackModel = NULL;
  
  for (i = 0; i < this->world->GetNumModels(); i++)
  {
    model = this->world->GetModels()[i];
    if (strcmp(model->GetId(), "car1") == 0) // HACK
      this->trackModel = model;
  }

  assert(this->trackModel);
  
  this->trackPose = this->trackModel->GetPose();
  */
  
  this->roadGeom->UpdateMesh(this->trackPose.pos);
    
  return 0;
}



//////////////////////////////////////////////////////////////////////////////
// Finalize the model
int Road::Fini()
{
  return 0;
}


//////////////////////////////////////////////////////////////////////////////
// Update model
void Road::Update(double step)
{
  /* TODO
  GzPose pose;
  GzVector d;

  if (!this->trackModel)
    return;

  pose = this->trackModel->GetPose();
  d = GzVectorSub(pose.pos, this->trackPose.pos);

  if (fabs(d.x) > 1.0 || fabs(d.y) > 1.0) // HACK
  {
    //this->roadGeom->UpdateMesh(pose.pos);
    this->trackPose = pose;
  }
  */
  
  return;
}