urdf2model.cpp

Go to the documentation of this file.

/*********************************************************************
* Software License Agreement (BSD License)
* 
*  Copyright (c) 2008, Willow Garage, Inc.
*  All rights reserved.
* 
*  Redistribution and use in source and binary forms, with or without
*  modification, are permitted provided that the following conditions
*  are met:
* 
*   * Redistributions of source code must retain the above copyright
*     notice, this list of conditions and the following disclaimer.
*   * Redistributions in binary form must reproduce the above
*     copyright notice, this list of conditions and the following
*     disclaimer in the documentation and/or other materials provided
*     with the distribution.
*   * Neither the name of the Willow Garage nor the names of its
*     contributors may be used to endorse or promote products derived
*     from this software without specific prior written permission.
* 
*  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
*  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
*  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
*  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
*  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
*  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
*  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
*  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
*  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
*  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
*  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
*  POSSIBILITY OF SUCH DAMAGE.
*********************************************************************/

#include <cstdio>
#include <cstdlib>

#include <vector>
#include <string>
#include <sstream>

#include "ros/ros.h"
#include <boost/program_options.hpp>

#include <gazebo/urdf2gazebo.h>

void usage(const char *progname)
{
    printf("\nUsage: %s [options]\n", progname);
    printf("  Example: %s --help\n",progname);
    printf("  Example: %s -f my_urdf_file.urdf -o gazebo_model.xml\n",progname);
}

namespace po = boost::program_options;

int main(int argc, char **argv)
{
  // print usage
  if (argc < 2)
  {
    usage(argv[0]);
    exit(1);
  }

  // Initial pose of model
  double initial_x = 0;
  double initial_y = 0;
  double initial_z = 0;
  double initial_rx = 0;
  double initial_ry = 0;
  double initial_rz = 0;

  // parse options
  po::options_description v_desc("Allowed options");
  v_desc.add_options()
    ("help,h" , "produce this help message")
    ("input-filename,f" , po::value<std::string>() , "read input model from file, not from the parameter server, exclusive with -p option.")
    ("output-filename,o" , po::value<std::string>() , "write converted gazebo model xml to a file instead, model is not spawned in simulation")
    ("model-name" , po::value<std::string>() , "Change name of Gazebo model")
    ("skip-joint-limits,s" , "do not enforce joint limits during urdf-->gazebo conversion.")
    ("init-x,x" , po::value<double>() , "set initial x position of model, defaults to 0.")
    ("init-y,y" , po::value<double>() , "set initial y position of model, defaults to 0.")
    ("init-z,z" , po::value<double>() , "set initial z position of model, defaults to 0.")
    ("init-roll,R" , po::value<double>() , "set initial roll (rx) of model in radians, defaults to 0. Application orders are r-p-y.")
    ("init-pitch,P" , po::value<double>() , "set initial pitch (ry) of model in radians, defaults to 0. Application orders are r-p-y.")
    ("init-yaw,Y" , po::value<double>() , "set initial yaw (rz) of model in radians, defaults to 0. Application orders are r-p-y.");

  
  po::options_description desc("Allowed options");
  desc.add(v_desc);
  
  po::positional_options_description p_desc;
  
  po::variables_map vm;
  po::store(po::command_line_parser(argc, argv).options(desc).positional(p_desc).run(), vm);
  po::notify(vm);
  
  if (vm.count("help"))
  {
    usage(argv[0]);
    std::cout << v_desc << std::endl;
    exit(1);
  }
  
  // set flag to enforce joint limits, this is hardcoded to true because we do want a model with
  // joint limits enforced.
  bool enforce_limits = true;
  if (vm.count("skip-joint-limits"))
    enforce_limits = false;
  
  std::string file_in;
  if (vm.count("input-filename"))
    file_in = vm["input-filename"].as<std::string>();
  else
    file_in.clear();
  
  std::string file_out;
  if (vm.count("output-filename"))
  {
    file_out = vm["output-filename"].as<std::string>();
  }  

  std::string model_name;
  if (vm.count("model-name"))
  {
    model_name = vm["model-namename"].as<std::string>();
  }  

  // Initial pose of model 
  if (vm.count("init-x"))
  {
    initial_x = vm["init-x"].as<double>();
    ROS_DEBUG("x: %f",initial_x);
  }
  if (vm.count("init-y"))
  {
    initial_y = vm["init-y"].as<double>();
    ROS_DEBUG("y: %f",initial_y);
  }
  if (vm.count("init-z"))
  {
    initial_z = vm["init-z"].as<double>();
    ROS_DEBUG("z: %f",initial_z);
  }
  if (vm.count("init-roll"))
  {
    initial_rx = vm["init-roll"].as<double>();
    ROS_DEBUG("init-roll: %f",initial_rx);
  }
  if (vm.count("init-pitch"))
  {
    initial_ry = vm["init-pitch"].as<double>();
    ROS_DEBUG("init-pitch: %f",initial_ry);
  }
  if (vm.count("init-yaw"))
  {
    initial_rz = vm["init-yaw"].as<double>();
    ROS_DEBUG("init-yaw: %f",initial_rz);
  }

  // setup initial pose vectors
  urdf::Vector3 initial_xyz(initial_x,           initial_y,           initial_z);
  urdf::Vector3 initial_rpy(initial_rx*180/M_PI, initial_ry*180/M_PI, initial_rz*180/M_PI);
      
  if (file_out.empty())
  {
    ROS_ERROR("Must specify output filename for gazebo model");
    exit(2);
  }

  // get input xml
  std::string xml_string;
  if (file_in.empty())
  {
    ROS_ERROR("Unable to load robot model from filename");
    exit(2);
  }

  // read urdf / gazebo model xml from file
  TiXmlDocument xml_in(file_in);
  xml_in.LoadFile();
  // copy tixml to a string
  std::ostringstream stream;
  stream << xml_in;
  xml_string = stream.str();
  
  // convert xml string to tinyxml object
  TiXmlDocument xml_doc;
  xml_doc.Parse(xml_string.c_str());

  // resulting gazebo model XML will be stored here
  TiXmlDocument gazebo_model_xml;
  
  // check root xml element to determine if conversion is necessary
  if (!xml_doc.FirstChild("robot"))
  {
    ROS_ERROR("Unable to determine input xml type: input urdf xml must begin with <robot>");
    exit(2);
  }
  
  // do convertion / manipulate xml
  // convert urdf to gazebo model
  urdf2gazebo::URDF2Gazebo u2g;
  u2g.convert(xml_doc, gazebo_model_xml, enforce_limits, initial_xyz, initial_rpy, model_name);
  
  // output filename is specified, save converted Gazebo Model XML to a file
  if (!gazebo_model_xml.SaveFile(file_out))
  {
    printf("Unable to save gazebo model in %s\n", file_out.c_str());  
    exit(3);
  }
  
  return 0;
}