spawn_gazebo_model.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 <gazebo/gazebo.h>
#include <gazebo/GazeboError.hh>
#include <gazebo/World.hh>
#include <gazebo/Model.hh>

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

#include <gazebo_tools/urdf2gazebo.h>

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

void remove_model(std::string remove_model_name)
{

  // connect to gazebo
  libgazebo::Client *client = new libgazebo::Client();
  libgazebo::FactoryIface *factoryIface = new libgazebo::FactoryIface();

  int serverId = 0;

  bool connected_to_server = false;
  while (!connected_to_server)
  {
    try
    {
      client->ConnectWait(serverId, GZ_CLIENT_ID_USER_FIRST);
      connected_to_server = true;
    }
    catch (gazebo::GazeboError e)
    {
      ROS_ERROR("Gazebo error: Unable to connect\n %s\n",e.GetErrorStr().c_str());
      usleep(1000000);
      connected_to_server = false;
    }
  }

  try
  {
    factoryIface->Open(client, "default");
  }
  catch (gazebo::GazeboError e)
  {
    ROS_ERROR("Gazebo error: Unable to connect to the factory interface\n%s\n",e.GetErrorStr().c_str());
    exit(2);
  }

  bool writing_iface = true;
  while (writing_iface)
  {
    factoryIface->Lock(1);
    if (strcmp((char*)factoryIface->data->deleteEntity,"")==0)
    {
      ROS_INFO("Deleting Robot Model Name:%s in Gazebo\n",remove_model_name.c_str());
      // don't overwrite data, only write if iface data is empty
      strcpy((char*)factoryIface->data->deleteEntity, remove_model_name.c_str());
      writing_iface = false;
    }
    factoryIface->Unlock();
  }
}

void spawn_model(TiXmlDocument gazebo_model_xml)
{
  //  Connect to Gazebo Iface Server and upload to Factory Iface
  libgazebo::Client *client = new libgazebo::Client();
  libgazebo::FactoryIface *factoryIface = new libgazebo::FactoryIface();

  int serverId = 0;

  bool connected_to_server = false;
  while (!connected_to_server)
  {
    try
    {
      ROS_DEBUG("spawn_gazebo_model waiting for gazebo factory, usually launched by 'roslaunch `rospack find gazebo`/launch/empty_world.launch'");
      client->ConnectWait(serverId, GZ_CLIENT_ID_USER_FIRST);
      connected_to_server = true;
    }
    catch (gazebo::GazeboError e)
    {
      ROS_ERROR("Gazebo error: Unable to connect\n %s\n",e.GetErrorStr().c_str());
      usleep(1000000);
      connected_to_server = false;
    }
  }

  // Open the Factory interface
  try
  {
    factoryIface->Open(client, "default");
  }
  catch (gazebo::GazeboError e)
  {
    ROS_ERROR("Gazebo error: Unable to connect to the factory interface\n%s\n",e.GetErrorStr().c_str());
    exit(2);
  }
  // Copy model to a string
  std::ostringstream stream;
  stream << gazebo_model_xml;
  std::string gazebo_model_xml_string = stream.str();
  ROS_DEBUG("Gazebo Model XML\n\n%s\n\n ",gazebo_model_xml_string.c_str());

  bool writing_iface = true;
  while (writing_iface)
  {
    factoryIface->Lock(1);
    if (strcmp((char*)factoryIface->data->newModel,"")==0)
    {
      // don't overwrite data, only write if iface data is empty
      strcpy((char*)factoryIface->data->newModel, gazebo_model_xml_string.c_str());
      writing_iface = false;
    }
    factoryIface->Unlock();
  }
}
namespace po = boost::program_options;

int main(int argc, char **argv)
{
    ros::init(argc,argv,"spawn_gazebo_model",ros::init_options::AnonymousName);

    ROS_WARN("spawn_gazebo_model will be deprecated in the next release, please use the new gazebo_model interface instead:\n\nrosrun gazebo_tools gazebo_model -h");

    // print usage
    if (argc < 2)
    {
        usage(argv[0]);
        exit(1);
    }

    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")
      ("model-name,m" , po::value<std::string>() , "overwrite name of the gazebo model in simulation. If not specified, the model name defaults to the name in urdf.")
      ("namespace,n" , po::value<std::string>() , "ROS namespace of the model spawned in simulation. If not specified, the namespace of this node is used.")
      ("file-in,i" , po::value<std::string>() , "read input model from file, not from the parameter server.  Do not specify param_name if an input filename is provided.")
      ("file-out,o" , po::value<std::string>() , "write converted gazebo model xml to a file instead, model is not spawned in simulation.")
      ("skip-joint-limits,l" , "do not enforce joint limits during urdf-->gazebo conversion.")
      ("init-x,x" , po::value<double>() , "set initial x position of model.")
      ("init-y,y" , po::value<double>() , "set initial y position of model.")
      ("init-z,z" , po::value<double>() , "set initial z position of model.")
      ("yaw,w" , po::value<double>() , "set initial yaw (rz) of model.  application orders are r-p-y.")
      ("pitch,p" , po::value<double>() , "set initial pitch (ry) of model.  application orders are r-p-y.")
      ("roll,r" , po::value<double>() , "set initial roll (rx) of model.  application orders are r-p-y.")
      ("remove,e" , po::value<std::string>() , "remove existing model in simulation (to be implemented)");

    po::options_description h_desc("Hidden options");
    h_desc.add_options()
      ("param-name" , po::value< std::vector<std::string> >(), "ROS param name containing URDF as a string.");

    po::options_description desc("Allowed options");
    desc.add(v_desc).add(h_desc);

    po::positional_options_description p_desc;
    p_desc.add("param-name", -1);

    po::variables_map vm;
    //po::store(po::parse_command_line(argc, argv, desc), 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;


    // if parameter contains urdf, convert from urdf to gazebo model
    bool do_conversion = true;

    std::string file_in;
    if (vm.count("file-in"))
      file_in = vm["file-in"].as<std::string>();
    else
      file_in.clear();

    std::string file_out;
    if (vm.count("file-out"))
    {
      file_out = vm["file-out"].as<std::string>();
    }

    std::string robot_namespace;
    if (vm.count("namespace"))
    {
      robot_namespace = vm["namespace"].as<std::string>();
    }
    else
    {
      robot_namespace = ros::this_node::getNamespace();
      ROS_DEBUG("NOTE: %s is called under namespace %s, this namespace is passed to gazebo so all Gazebo plugins (e.g.ros_time, gazebo_mechanism_control,etc.) will start up under this namespace.  User can overwrite by passing in argument -namespace, for example: %s /prf/robot_description -n /prf",argv[0],argv[0],robot_namespace.c_str());
    }
    ROS_DEBUG("namespace: %s",robot_namespace.c_str());


    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("roll"))
    {
      initial_rx = vm["roll"].as<double>();
      ROS_DEBUG("roll: %f",initial_rx);
    }
    if (vm.count("pitch"))
    {
      initial_ry = vm["pitch"].as<double>();
      ROS_DEBUG("pitch: %f",initial_ry);
    }
    if (vm.count("yaw"))
    {
      initial_rz = vm["yaw"].as<double>();
      ROS_DEBUG("yaw: %f",initial_rz);
    }

    std::string remove_model_name;
    if (vm.count("remove"))
    {
      remove_model_name = vm["remove"].as<std::string>();
      ROS_DEBUG("remove: %s",remove_model_name.c_str());
    }
    else
      remove_model_name.clear();

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

    std::string param_name;
    if (vm.count("param-name"))
    {
      if (!file_in.empty())
      {
        ROS_ERROR("Both param-name and input file name are specified.  Only one input source is allowed.\n");  
        exit(2);
      }

      std::vector<std::string> str_vec = vm["param-name"].as<std::vector<std::string> >();
      if (str_vec.size() > 1)
      {
        ROS_WARN("multiple urdf is deprecated!");
        param_name = str_vec[0];
        if (str_vec.size() >= 4)
        {
          initial_x = atof(str_vec[1].c_str());
          initial_y = atof(str_vec[2].c_str());
          initial_z = atof(str_vec[3].c_str());
        }
        if (str_vec.size() >= 7)
        {
          initial_rx = atof(str_vec[4].c_str());
          initial_ry = atof(str_vec[5].c_str());
          initial_rz = atof(str_vec[6].c_str());
        }
        if (str_vec.size() >= 8)
          model_name = str_vec[7];
      }
      else
      {
        ROS_DEBUG("parameter names are: ");
        for (std::vector<std::string>::iterator str = str_vec.begin(); str != str_vec.end(); str++)
        {
          ROS_DEBUG("  %s",str->c_str());
          param_name = *str;
        }
      }
    }

    // get rid of slahses in robot model name
    std::replace(model_name.begin(),model_name.end(),'/','_');
    ROS_DEBUG("model name will be: %s",model_name.c_str());

    // setup initial pose vectors
    urdf::Vector3 initial_xyz(initial_x,initial_y,initial_z);
    urdf::Vector3 initial_rpy(initial_rx,initial_ry,initial_rz);

    if (!remove_model_name.empty())
    {
      //   remove model
      remove_model(remove_model_name);
    }
    else // spawn model
    {
      // get input xml
      std::string xml_string;
      if (file_in.empty())
      {
        // startup an node handle, load parameter server string for robot/model
        ros::NodeHandle rosnode("~");
        std::string full_param_name;
        rosnode.searchParam(param_name,full_param_name);
        rosnode.getParam(full_param_name.c_str(),xml_string);
        ROS_DEBUG("%s content\n%s\n", full_param_name.c_str(), xml_string.c_str());
        if (xml_string.c_str()==NULL)
        {
            ROS_ERROR("Unable to load robot model from param server robot_description\n");  
            exit(2);
        }
      }
      else
      {
        // 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"))
        do_conversion = true;
      else if (xml_doc.FirstChild("model:physical"))
        do_conversion = false;
      else
      {
          ROS_ERROR("Unable to determine input xml type: input urdf xml must begin with <robot>, input gazebo model xml must begin with <model:physical>\n");  
          exit(2);
      }

      // do convertion / manipulate xml
      if (do_conversion)
      {
        // convert urdf to gazebo model
        urdf2gazebo::URDF2Gazebo u2g;
        u2g.convert(xml_doc, gazebo_model_xml, enforce_limits, initial_xyz, initial_rpy, model_name, robot_namespace);
      }
      else
      {
        std::string open_bracket("<?");
        std::string close_bracket("?>");
        int pos1 = xml_string.find(open_bracket,0);
        int pos2 = xml_string.find(close_bracket,0);
        xml_string.replace(pos1,pos2-pos1+2,std::string(""));

        // put string in TiXmlDocument for manipulation
        gazebo_model_xml.Parse(xml_string.c_str());

        // optional model manipulations:
        //  * update initial pose
        //  * replace model name
        TiXmlElement* model;
        model = gazebo_model_xml.FirstChildElement("model:physical");
        if (model)
        {
          // replace initial pose of robot
          // find first instance of xyz and rpy, replace with initial pose
          TiXmlElement* xyz_key = model->FirstChildElement("xyz");
          if (xyz_key)
            model->RemoveChild(xyz_key);
          TiXmlElement* rpy_key = model->FirstChildElement("rpy");
          if (rpy_key)
            model->RemoveChild(rpy_key);

          xyz_key = new TiXmlElement("xyz");
          rpy_key = new TiXmlElement("rpy");

          std::ostringstream xyz_stream, rpy_stream;
          xyz_stream << initial_x << " " << initial_y << " " << initial_z;
          rpy_stream << initial_rx << " " << initial_ry << " " << initial_rz;

          TiXmlText* xyz_txt = new TiXmlText(xyz_stream.str());
          TiXmlText* rpy_txt = new TiXmlText(rpy_stream.str());

          xyz_key->LinkEndChild(xyz_txt);
          rpy_key->LinkEndChild(rpy_txt);

          model->LinkEndChild(xyz_key);
          model->LinkEndChild(rpy_key);


          // replace model name if one is specified by the user
          if (!model_name.empty())
          {
            // replace model name by the one specified by user
            model->RemoveAttribute("name");
            model->SetAttribute("name",model_name);
          }
          else
          {
            // set model_name to name specified in XML
            model_name = model->Attribute("name");
          }

        }

      }

      if (file_out.empty())
      {
        // send model to FactoryIface
        spawn_model(gazebo_model_xml);
      }
      else
      {
        // 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;
}

---

gazebo_tools
Author(s): John Hsu
autogenerated on Fri Jan 11 09:40:43 2013