gz.cc

/*
 * Copyright (C) 2014 Open Source Robotics Foundation
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *     http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 *
*/
#include <stdio.h>
#include <signal.h>
#include <tinyxml.h>
#include <boost/filesystem.hpp>
#include <boost/algorithm/string.hpp>
#include <streambuf>

#include <gazebo/common/common.hh>
#include <gazebo/transport/transport.hh>
#include <sdf/sdf.hh>
#include "gz_log.hh"
#include "gz_marker.hh"
#include "gz_topic.hh"
#include "gz.hh"

using namespace gazebo;

boost::mutex Command::sigMutex;
boost::condition_variable Command::sigCondition;

std::map<std::string, Command *> g_commandMap;

/////////////////////////////////////////////////
Command::Command(const std::string &_name, const std::string &_brief)
  : name(_name), brief(_brief), visibleOptions("Options"), argc(0), argv(NULL)
{
  this->visibleOptions.add_options()
    ("verbose", "Print extra information")
    ("help,h", "Print this help message");
}

/////////////////////////////////////////////////
Command::~Command()
{
  delete [] this->argv;
  this->argv = NULL;
}

/////////////////////////////////////////////////
void Command::Signal()
{
  boost::mutex::scoped_lock lock(sigMutex);
  sigCondition.notify_all();
}

/////////////////////////////////////////////////
void Command::ListOptions()
{
  std::vector<std::string> pieces;

  std::vector<boost::shared_ptr<po::option_description> >::const_iterator iter;
  for (iter = this->visibleOptions.options().begin();
      iter != this->visibleOptions.options().end(); ++iter)
  {
    pieces.clear();
    std::string formatName = (*iter)->format_name();
    pieces = common::split(formatName, " ");

    if (pieces.empty())
    {
      std::cerr << "Unable to process list options.\n";
      return;
    }

    // Output the short name option, or long name if there is no shortname
    std::cout << pieces[0] << std::endl;

    // Output the long name option if it exists.
    if (pieces.size() > 3)
      std::cout << pieces[2] << std::endl;
  }
}

/////////////////////////////////////////////////
void Command::Help()
{
  std::cerr << " gz " << this->name << " [options]\n\n";
  this->HelpDetailed();
  std::cerr << this->visibleOptions << "\n";
}

/////////////////////////////////////////////////
std::string Command::GetBrief() const
{
  return this->brief;
}

/////////////////////////////////////////////////
bool Command::TransportInit()
{
  // Some command require transport, and some do not. Only initialize
  // transport if required.
  if (!this->TransportRequired())
    return true;

  // Setup transport (communication)
  if (!transport::init("", 0, 1))
    return false;

  // Run transport (communication)
  transport::run();

  return true;
}

/////////////////////////////////////////////////
bool Command::TransportRequired()
{
  return true;
}

/////////////////////////////////////////////////
bool Command::TransportFini()
{
  transport::fini();
  return true;
}

/////////////////////////////////////////////////
bool Command::Run(int _argc, char **_argv)
{
  // save a copy of argc and argv for consumption by child commands
  this->argc = _argc;
  this->argv = new char*[_argc];
  for (int i = 0; i < _argc; ++i)
  {
    int argvLen = strlen(_argv[i]) + 1;
    this->argv[i] = new char[argvLen];
    #ifndef _WIN32
    snprintf(this->argv[i], argvLen, "%s", _argv[i]);
    #else
    sprintf_s(this->argv[i], argvLen, "%s", _argv[i]);
    #endif
  }

  // The SDF find file callback.
  using namespace boost::placeholders;
  sdf::setFindCallback(boost::bind(&gazebo::common::find_file, _1));

  // Hidden options
  po::options_description hiddenOptions("hidden options");
  hiddenOptions.add_options()
    ("command", po::value<std::string>(), "Command")
    ("pass", po::value<std::vector<std::string> >(), "pass through");

  po::options_description allOptions("all options");
  allOptions.add(hiddenOptions).add(this->visibleOptions);

  // The command and file options are positional
  po::positional_options_description positional;
  positional.add("command", 1).add("pass", -1);

  try
  {
    po::store(
        po::command_line_parser(_argc, _argv).options(allOptions).positional(
          positional).run(), this->vm);
    po::notify(this->vm);
  }
  catch(boost::exception &_e)
  {
    std::cerr << "Invalid arguments\n";
    return false;
  }

  if (this->vm.count("help"))
  {
    this->Help();
    return true;
  }

  if (this->vm.count("verbose"))
  {
    gazebo::common::Console::SetQuiet(false);
  }

  if (!this->TransportInit())
  {
    std::cerr << "An instance of Gazebo is not running.\n";
    return false;
  }

  bool result = this->RunImpl();

  this->TransportFini();

  return result;
}

/////////////////////////////////////////////////
WorldCommand::WorldCommand()
  : Command("world", "Modify world properties")
{
  // Options that are visible to the user through help.
  this->visibleOptions.add_options()
    ("world-name,w", po::value<std::string>(), "World name.")
    ("pause,p", po::value<bool>(), "Pause/unpause simulation. "
     "0=unpause, 1=pause.")
    ("step,s", "Step simulation one iteration.")
    ("multi-step,m", po::value<uint32_t>(),
     "Step simulation mulitple iteration.")
    ("reset-all,r", "Reset time and model poses")
    ("reset-time,t", "Reset time")
    ("reset-models,o", "Reset models");
}

/////////////////////////////////////////////////
void WorldCommand::HelpDetailed()
{
  std::cerr <<
    "\tChange properties of a Gazebo world on a running\n "
    "\tserver. If a name for the world, option -w, is not specified\n"
    "\tthe first world found on the Gazebo master will be used.\n"
    << std::endl;
}

/////////////////////////////////////////////////
bool WorldCommand::RunImpl()
{
  std::string worldName;

  if (this->vm.count("world-name"))
    worldName = this->vm["world-name"].as<std::string>();

  transport::NodePtr node(new transport::Node());
  node->Init(worldName);

  transport::PublisherPtr pub =
    node->Advertise<msgs::WorldControl>("~/world_control");
  pub->WaitForConnection();

  msgs::WorldControl msg;
  bool good = false;

  if (this->vm.count("pause"))
  {
    msg.set_pause(this->vm["pause"].as<bool>());
    good = true;
  }

  if (this->vm.count("step"))
  {
    msg.set_step(true);
    good = true;
  }

  if (this->vm.count("multi-step"))
  {
    msg.set_multi_step(this->vm["multi-step"].as<uint32_t>());
    good = true;
  }

  if (this->vm.count("reset-all"))
  {
    msg.mutable_reset()->set_all(true);
    good = true;
  }

  if (this->vm.count("reset-time"))
  {
    msg.mutable_reset()->set_time_only(true);
    good = true;
  }

  if (this->vm.count("reset-models"))
  {
    msg.mutable_reset()->set_model_only(true);
    good = true;
  }

  if (good)
    pub->Publish(msg, true);
  else
    this->Help();

  return true;
}

/////////////////////////////////////////////////
PhysicsCommand::PhysicsCommand()
  : Command("physics", "Modify properties of the physics engine")
{
  // Options that are visible to the user through help.
  this->visibleOptions.add_options()
    ("world-name,w", po::value<std::string>(), "World name.")
    ("gravity,g", po::value<std::string>(),
     "Gravity vector. Comma separated 3-tuple without whitespace, "
     "eg: -g 0,0,-9.8")
    ("step-size,s", po::value<double>(), "Maximum step size (seconds).")
    ("iters,i", po::value<double>(), "Number of iterations.")
    ("update-rate,u", po::value<double>(), "Target real-time update rate.")
    ("profile,o", po::value<std::string>(), "Preset physics profile.");
}

/////////////////////////////////////////////////
void PhysicsCommand::HelpDetailed()
{
  std::cerr <<
    "\tChange properties of the physics engine on a specific\n"
    "\tworld. If a name for the world, option -w, is not specified,\n"
    "\tthe first world found on the Gazebo master will be used.\n"
    << std::endl;
}

/////////////////////////////////////////////////
bool PhysicsCommand::RunImpl()
{
  std::string worldName;

  if (this->vm.count("world-name"))
    worldName = this->vm["world-name"].as<std::string>();

  transport::NodePtr node(new transport::Node());
  node->Init(worldName);

  transport::PublisherPtr pub =
    node->Advertise<msgs::Physics>("~/physics");
  pub->WaitForConnection();

  msgs::Physics msg;

  bool good = false;

  if (this->vm.count("step-size"))
  {
    msg.set_max_step_size(this->vm["step-size"].as<double>());
    good = true;
  }

  if (this->vm.count("iters"))
  {
    msg.set_iters(this->vm["iters"].as<double>());
    good = true;
  }

  if (this->vm.count("update-rate"))
  {
    msg.set_real_time_update_rate(this->vm["update-rate"].as<double>());
    good = true;
  }

  if (this->vm.count("gravity"))
  {
    auto values = common::split(this->vm["gravity"].as<std::string>(), ",");

    msg.mutable_gravity()->set_x(boost::lexical_cast<double>(values[0]));
    msg.mutable_gravity()->set_y(boost::lexical_cast<double>(values[1]));
    msg.mutable_gravity()->set_z(boost::lexical_cast<double>(values[2]));
    good = true;
  }

  if (this->vm.count("profile") &&
      this->vm["profile"].as<std::string>().size() > 0)
  {
    msg.set_profile_name(this->vm["profile"].as<std::string>());
    good = true;
  }

  if (good)
    pub->Publish(msg, true);
  else
    this->Help();

  return true;
}

/////////////////////////////////////////////////
ModelCommand::ModelCommand()
  : Command("model", "Modify properties of a model")
{
  // Options that are visible to the user through help.
  this->visibleOptions.add_options()
    ("model-name,m", po::value<std::string>(), "Model name.")
    ("world-name,w", po::value<std::string>(), "World name.")
    ("delete,d", "Delete a model.")
    ("spawn-file,f", po::value<std::string>(), "Spawn model from SDF file.")
    ("spawn-string,s", "Spawn model from SDF string, pass by a pipe.")
    ("info,i", "Output model state information to the terminal.")
    ("pose,p",
     "Output model pose as a space separated 6-tuple: x y z roll pitch yaw.")
    ("pose-x,x", po::value<double>(), "x value")
    ("pose-y,y", po::value<double>(), "y value")
    ("pose-z,z", po::value<double>(), "z value")
    ("pose-R,R", po::value<double>(), "roll in radians.")
    ("pose-P,P", po::value<double>(), "pitch in radians.")
    ("pose-Y,Y", po::value<double>(), "yaw in radians.");
}

/////////////////////////////////////////////////
void ModelCommand::HelpDetailed()
{
  std::cerr <<
    "\tChange properties of a model, delete a model, or\n"
    "\tspawn a new model. If a name for the world, option -w, is\n"
    "\tnot pecified, the first world found on the Gazebo master\n"
    "\twill be used.\n"
    << std::endl;
}

/////////////////////////////////////////////////
bool ModelCommand::RunImpl()
{
  std::string modelName, worldName;

  if (this->vm.count("world-name"))
    worldName = this->vm["world-name"].as<std::string>();

  if (this->vm.count("model-name"))
    modelName = this->vm["model-name"].as<std::string>();
  else
  {
    std::cerr << "A model name is required using the "
      << "(-m <model_name> command line argument)\n";
    std::cerr << "For more information: gz help model.\n";
    return false;
  }

  ignition::math::Pose3d pose;
  ignition::math::Vector3d rpy;

  if (this->vm.count("pose-x"))
    pose.Pos().X(this->vm["pose-x"].as<double>());
  if (this->vm.count("pose-y"))
    pose.Pos().Y(this->vm["pose-y"].as<double>());
  if (this->vm.count("pose-z"))
    pose.Pos().Z(this->vm["pose-z"].as<double>());
  if (this->vm.count("pose-R"))
    rpy.X(this->vm["pose-R"].as<double>());
  if (this->vm.count("pose-P"))
    rpy.Y(this->vm["pose-P"].as<double>());
  if (this->vm.count("pose-Y"))
    rpy.Z(this->vm["pose-Y"].as<double>());
  pose.Rot().Euler(rpy);

  transport::NodePtr node(new transport::Node());
  node->Init(worldName);

  if (this->vm.count("delete"))
  {
    msgs::Request *msg = msgs::CreateRequest("entity_delete", modelName);
    transport::PublisherPtr pub = node->Advertise<msgs::Request>("~/request");
    pub->WaitForConnection();
    pub->Publish(*msg, true);
    delete msg;
  }
  else if (this->vm.count("spawn-file"))
  {
    std::string filename = this->vm["spawn-file"].as<std::string>();

    std::ifstream ifs(filename.c_str());
    if (!ifs)
    {
      std::cerr << "Error: Unable to open file[" << filename << "]\n";
      return false;
    }

    sdf::SDFPtr sdf(new sdf::SDF());
    if (!sdf::init(sdf))
    {
      std::cerr << "Error: SDF parsing the xml failed" << std::endl;
      return false;
    }

    sdf::Errors errors;
    if (!sdf::readFileWithoutConversion(filename, sdf, errors))
    {
      std::cerr << "Error: SDF parsing the xml failed\n";
      return false;
    }

    std::string resolvedFileName = filename;
    if (common::isDirectory(filename))
    {
      // Use sdf::getModelFilePath() instead of filename because filename
      // might be a model rectory
      resolvedFileName = sdf::getModelFilePath(filename);
      if (resolvedFileName.empty())
      {
        std::cerr << "The provided file is a directory, but a model could not "
                  << "be found" << std::endl;
      }
    }
    std::ifstream sdfFile(resolvedFileName);

    // Using const std::string sdfString(arg, arg) confuses the compiler, so use
    // move assignment
    const auto sdfString = std::string(std::istreambuf_iterator<char>(sdfFile),
                                       std::istreambuf_iterator<char>());
    return this->ProcessSpawn(sdfString, modelName, pose, node);
  }
  else if (this->vm.count("spawn-string"))
  {
    std::string input;
    std::string sdfString;

    // Read input from the command line.
    while (std::getline(std::cin, input))
    {
      sdfString += input;
    }

    sdf::SDFPtr sdf(new sdf::SDF());
    if (!sdf::init(sdf))
    {
      std::cerr << "Error: SDF parsing the xml failed" << std::endl;
      return false;
    }

    sdf::Errors errors;
    if (!sdf::readStringWithoutConversion(sdfString, sdf, errors))
    {
      std::cerr << "Error: SDF parsing the xml failed\n";
      return false;
    }

    return this->ProcessSpawn(sdfString, modelName, pose, node);
  }
  else if (this->vm.count("info") || this->vm.count("pose"))
  {
    boost::shared_ptr<msgs::Response> response = gazebo::transport::request(
        worldName, "entity_info", modelName);
    gazebo::msgs::Model modelMsg;

    if (response->has_serialized_data() &&
        !response->serialized_data().empty() &&
        modelMsg.ParseFromString(response->serialized_data()))
    {
      if (this->vm.count("info"))
        std::cout << modelMsg.DebugString() << std::endl;
      else if (this->vm.count("pose"))
        std::cout << gazebo::msgs::ConvertIgn(modelMsg.pose()) << std::endl;
    }
    else
    {
      std::string tmpWorldName = worldName.empty() ? "default" : worldName;
      std::cout << "Unable to get info on model[" << modelName << "] in "
        << "the world[" << tmpWorldName << "]\n";
    }
  }
  else
  {
    transport::PublisherPtr pub =
      node->Advertise<msgs::Model>("~/model/modify");
    pub->WaitForConnection();

    msgs::Model msg;
    msg.set_name(modelName);
    msgs::Set(msg.mutable_pose(), pose);
    pub->Publish(msg, true);
  }

  return true;
}

/////////////////////////////////////////////////
bool ModelCommand::ProcessSpawn(const std::string &_sdfString,
    const std::string &_name, const ignition::math::Pose3d &_pose,
    transport::NodePtr _node)
{
  transport::PublisherPtr pub = _node->Advertise<msgs::Factory>("~/factory");
  pub->WaitForConnection();

  // To set the name of the model, we have to parse the file, set the name and
  // reserialize to SDFormat. However, loading it using libsdformat will result
  // in upconversion, which we would like to avoid because the resulting
  // document will lose its OriginalVersion attribute. So, instead, we parse and
  // edit the model name with TinyXML.

  TiXmlDocument doc;
  doc.Parse(_sdfString.c_str());
  auto *root = doc.RootElement();
  if (nullptr == root)
  {
    std::cerr << "Invalid XML file" << std::endl;
    std::cout << _sdfString << std::endl;
    return false;
  }

  // Handle SDFormat and URDF separately
  if (root->ValueStr() == "sdf")
  {
    auto *model = root->FirstChildElement("model");
    if (nullptr != model)
    {
      model->SetAttribute("name", _name);
    }
    else
    {
      std::cerr << "Could not find <model> tag in SDFormat file" << std::endl;
    }
  }
  else if (root->ValueStr() == "robot")
  {
    root->SetAttribute("name", _name);
  }
  else
  {
    std::cerr << "Unknown file format" << std::endl;
  }

  TiXmlPrinter printer;
  doc.Accept(&printer);
  msgs::Factory msg;
  msg.set_sdf(printer.Str());
  msgs::Set(msg.mutable_pose(), _pose);
  pub->Publish(msg, true);

  return true;
}


/////////////////////////////////////////////////
JointCommand::JointCommand()
  : Command("joint", "Modify properties of a joint")
{
  // Options that are visible to the user through help.
  this->visibleOptions.add_options()
    ("world-name,w", po::value<std::string>(), "World name.")
    ("model-name,m", po::value<std::string>(), "Model name.")
    ("joint-name,j", po::value<std::string>(), "Joint name.")
    ("force,f", po::value<double>(), "Force to apply to a joint (N).")
    ("pos-t", po::value<double>(),
     "Target angle(rad) for rotation joints or position (m) for linear joints.")
    ("pos-p", po::value<double>(), "Position proportional gain.")
    ("pos-i", po::value<double>(), "Position integral gain.")
    ("pos-d", po::value<double>(), "Position differential gain.")
    ("vel-t", po::value<double>(),
     "Target speed (rad/s for rotational joints or m/s for linear joints).")
    ("vel-p", po::value<double>(), "Velocity proportional gain.")
    ("vel-i", po::value<double>(), "Velocity integral gain.")
    ("vel-d", po::value<double>(), "Velocity differential gain.");
}

/////////////////////////////////////////////////
void JointCommand::HelpDetailed()
{
  std::cerr <<
    "\tChange properties of a joint. If a name for the world, \n"
    "\toption -w, is not specified, the first world found on \n"
    "\tthe Gazebo master will be used.\n"
    "\tA model name and joint name are required.\n"
    "\n"
    "\tIt is recommended to use only one type of command:\n"
    "\tforce, position PID, or velocity PID.\n"
    "\n"
    "\tForce: Use --force to apply a force.\n"
    "\n"
    "\tPosition PID: Use --pos-t to specify a target position\n"
    "\twith --pos-p, --pos-i, --pos-d to specify the PID parameters.\n"
    "\n"
    "\tVelocity PID: Use --vel-t to specify a target velocity\n"
    "\twith --vel-p, --vel-i, --vel-d to specify the PID parameters.\n"
    << std::endl;
}

/////////////////////////////////////////////////
bool JointCommand::RunImpl()
{
  std::string modelName, jointName;

  if (this->vm.count("model-name"))
    modelName = this->vm["model-name"].as<std::string>();
  else
  {
    std::cerr << "A model name is required using the "
      << "(-m <model_name> command line argument)\n";
    std::cerr << "For more information: gz help joint.\n";
    return false;
  }

  if (this->vm.count("joint-name"))
    jointName = this->vm["joint-name"].as<std::string>();
  else
  {
    std::cerr << "A joint name is required using the "
      << "(-j <joint_name> command line argument)\n";
    std::cerr << "For more information: gz help joint.\n";
    return false;
  }

  ignition::msgs::JointCmd msg;
  msg.set_name(modelName + "::" + jointName);

  if (this->vm.count("force"))
    msg.mutable_force_optional()->set_data(this->vm["force"].as<double>());

  if (this->vm.count("pos-t"))
  {
    msg.mutable_position()->mutable_target_optional()->set_data(
        this->vm["pos-t"].as<double>());

    if (this->vm.count("pos-p"))
    {
      msg.mutable_position()->mutable_p_gain_optional()->set_data(
          this->vm["pos-p"].as<double>());
    }

    if (this->vm.count("pos-i"))
    {
      msg.mutable_position()->mutable_i_gain_optional()->set_data(
          this->vm["pos-i"].as<double>());
    }

    if (this->vm.count("pos-d"))
    {
      msg.mutable_position()->mutable_d_gain_optional()->set_data(
          this->vm["pos-d"].as<double>());
    }
  }

  if (this->vm.count("vel-t"))
  {
    msg.mutable_velocity()->mutable_target_optional()->set_data(
        this->vm["vel-t"].as<double>());

    if (this->vm.count("vel-p"))
    {
      msg.mutable_velocity()->mutable_p_gain_optional()->set_data(
          this->vm["vel-p"].as<double>());
    }

    if (this->vm.count("vel-i"))
    {
      msg.mutable_velocity()->mutable_i_gain_optional()->set_data(
          this->vm["vel-i"].as<double>());
    }

    if (this->vm.count("vel-d"))
    {
      msg.mutable_velocity()->mutable_d_gain_optional()->set_data(
          this->vm["vel-d"].as<double>());
    }
  }

  std::string topic = std::string("/") + modelName + "/joint_cmd";

  ignition::transport::Node ignNode;
  auto pub = ignNode.Advertise<ignition::msgs::JointCmd>(topic);

  unsigned int maxSleep = 30;
  unsigned int sleep = 0;
  unsigned int mSleep = 100;
  for (; sleep < maxSleep && !pub.HasConnections(); ++sleep)
  {
    common::Time::MSleep(mSleep);
  }

  if (sleep == maxSleep)
  {
    gzerr << "No subscribers to topic [" << topic <<"], timed out after " <<
        maxSleep * mSleep << "ms." << std::endl;
    return false;
  }

  pub.Publish(msg);

  return true;
}

/////////////////////////////////////////////////
CameraCommand::CameraCommand()
  : Command("camera", "Control a camera")
{
  // Options that are visible to the user through help.
  this->visibleOptions.add_options()
    ("world-name,w", po::value<std::string>(), "World name.")
    ("camera-name,c", po::value<std::string>(),
     "Camera name. Use gz camera -l to get a list of camera names.")
    ("list,l", "List all cameras")
    ("follow,f", po::value<std::string>(), "Model to follow.");
}

/////////////////////////////////////////////////
void CameraCommand::HelpDetailed()
{
  std::cerr <<
    "\tChange properties of a camera. If a name for the world, \n"
    "\toption -w, is not specified, the first world found on \n"
    "\tthe Gazebo master will be used.\n"
    "\tA camera name is required.\n"
    << std::endl;
}

/////////////////////////////////////////////////
bool CameraCommand::RunImpl()
{
  std::string cameraName, worldName;

  if (this->vm.count("world-name"))
    worldName = this->vm["world-name"].as<std::string>();

  if (this->vm.count("list"))
  {
    transport::ConnectionPtr connection = transport::connectToMaster();

    if (connection)
    {
      msgs::Packet packet;
      msgs::Request *request;
      msgs::GzString_V topics;
      std::string data;

      request = msgs::CreateRequest("get_topics");
      request->set_id(0);
      connection->EnqueueMsg(msgs::Package("request", *request), true);
      connection->Read(data);

      packet.ParseFromString(data);
      topics.ParseFromString(packet.serialized_data());

      for (int i = 0; i < topics.data_size(); ++i)
      {
        request = msgs::CreateRequest("topic_info", topics.data(i));
        connection->EnqueueMsg(msgs::Package("request", *request), true);

        int j = 0;
        do
        {
          connection->Read(data);
          packet.ParseFromString(data);
        } while (packet.type() != "topic_info_response" && ++j < 10);

        msgs::TopicInfo topicInfo;

        if (j <10)
          topicInfo.ParseFromString(packet.serialized_data());
        else
        {
          std::cerr << "Unable to get info for topic["
                    << topics.data(i) << "]\n";
        }

        if (topicInfo.msg_type() == "gazebo.msgs.CameraCmd")
        {
          auto parts = common::split(topics.data(i), "/");
          std::cout << parts[parts.size()-2] << std::endl;
        }
      }
    }
    else
    {
      std::cerr << "Unable to connect to a running instance of gazebo.\n";
    }

    return true;
  }


  if (this->vm.count("camera-name"))
    cameraName = this->vm["camera-name"].as<std::string>();
  else
  {
    std::cerr << "A camera name is required using the "
      << "(-c <camera_name> command line argument)\n";
    std::cerr << "For more information: gz help camera\n";
    return false;
  }

  msgs::CameraCmd msg;
  if (this->vm.count("follow"))
    msg.set_follow_model(this->vm["follow"].as<std::string>());

  transport::NodePtr node(new transport::Node());
  node->Init(worldName);

  boost::replace_all(cameraName, "::", "/");

  transport::PublisherPtr pub =
    node->Advertise<msgs::CameraCmd>(
        std::string("~/") + cameraName + "/cmd");

  pub->WaitForConnection();

  pub->Publish(msg, true);

  return true;
}

/////////////////////////////////////////////////
StatsCommand::StatsCommand()
  : Command("stats", "Print statistics about a running gzserver instance.")
{
  // Options that are visible to the user through help.
  this->visibleOptions.add_options()
    ("world-name,w", po::value<std::string>(), "World name.")
    ("duration,d", po::value<uint64_t>(), "Duration (seconds) to run.")
    ("plot,p", "Output comma-separated values, useful for processing and "
     "plotting.");
}

/////////////////////////////////////////////////
void StatsCommand::HelpDetailed()
{
  std::cerr <<
    "\tPrint gzserver statics to standard out. If a name for the world, \n"
    "\toption -w, is not specified, the first world found on \n"
    "\tthe Gazebo master will be used.\n"
    << std::endl;
}

/////////////////////////////////////////////////
bool StatsCommand::RunImpl()
{
  std::string worldName;

  if (this->vm.count("world-name"))
    worldName = this->vm["world-name"].as<std::string>();

  transport::NodePtr node(new transport::Node());
  node->Init(worldName);

  transport::SubscriberPtr sub =
    node->Subscribe("~/world_stats", &StatsCommand::CB, this);

  boost::mutex::scoped_lock lock(this->sigMutex);
  if (this->vm.count("duration"))
    this->sigCondition.timed_wait(lock,
        boost::posix_time::seconds(this->vm["duration"].as<uint64_t>()));
  else
    this->sigCondition.wait(lock);

  return true;
}

/////////////////////////////////////////////////
void StatsCommand::CB(ConstWorldStatisticsPtr &_msg)
{
  GZ_ASSERT(_msg, "Invalid message received");

  double percent = 0;
  char paused;
  common::Time simTime  = msgs::Convert(_msg->sim_time());
  common::Time realTime = msgs::Convert(_msg->real_time());

  this->simTimes.push_back(msgs::Convert(_msg->sim_time()));
  if (this->simTimes.size() > 20)
    this->simTimes.pop_front();

  this->realTimes.push_back(msgs::Convert(_msg->real_time()));
  if (this->realTimes.size() > 20)
    this->realTimes.pop_front();

  common::Time simAvg, realAvg;
  std::list<common::Time>::iterator simIter, realIter;
  simIter = ++(this->simTimes.begin());
  realIter = ++(this->realTimes.begin());
  while (simIter != this->simTimes.end() && realIter != this->realTimes.end())
  {
    simAvg += ((*simIter) - this->simTimes.front());
    realAvg += ((*realIter) - this->realTimes.front());
    ++simIter;
    ++realIter;
  }

  // Prevent divide by zero
  if (realAvg <= 0)
    return;

  simAvg = simAvg / realAvg;

  if (simAvg > 0)
    percent = simAvg.Double();
  else
    percent = 0;


  if (_msg->paused())
    paused = 'T';
  else
    paused = 'F';

  if (this->vm.count("plot"))
  {
    static bool first = true;
    if (first)
    {
      std::cout << "# real-time factor (percent), simtime (sec), "
        << "realtime (sec), paused (T or F)\n";
      first = false;
    }
    printf("%4.2f, %16.6f, %16.6f, %c\n",
        percent, simTime.Double(), realTime.Double(), paused);
    fflush(stdout);
  }
  else
    printf("Factor[%4.2f] SimTime[%4.2f] RealTime[%4.2f] Paused[%c]\n",
        percent, simTime.Double(), realTime.Double(), paused);
}

/////////////////////////////////////////////////
SDFCommand::SDFCommand()
  : Command("sdf",
      "Converts between SDF versions, and provides info about SDF files")
{
  // Options that are visible to the user through help.
  this->visibleOptions.add_options()
    ("describe,d", "Print SDF format for given version(-v).")
    ("convert,c", po::value<std::string>(),
     "In place conversion of arg to the latest SDF version.")
    ("doc,o", "Print HTML SDF. Use -v to specify version.")
    ("check,k", po::value<std::string>(), "Validate arg.")
    ("version,v", po::value<std::string>(),
     "Version of SDF to use with other options.")
    ("print,p", po::value<std::string>(),
     "Print arg, useful for debugging and as a conversion tool.");
}

/////////////////////////////////////////////////
void SDFCommand::HelpDetailed()
{
  std::cerr <<
    "\tIntrospect, convert, and output SDF files.\n"
    "\tUse the -v option to specify the version of\n"
    "\tSDF for use with other options.\n"
    << std::endl;
}

/////////////////////////////////////////////////
bool SDFCommand::TransportRequired()
{
  return false;
}

/////////////////////////////////////////////////
bool SDFCommand::RunImpl()
{
  sdf::SDF::Version(SDF_VERSION);

  try
  {
    // Initialize the informational logger. This will log warnings and errors.
    gzLogInit("gz-", "gzsdf.log");
  }
  catch(gazebo::common::Exception &_e)
  {
    _e.Print();
    std::cerr << "Error initializing log file" << std::endl;
  }

  sdf::SDFPtr sdf(new sdf::SDF());

  if (this->vm.count("version"))
  {
    try
    {
      sdf::SDF::Version(boost::lexical_cast<std::string>(
          this->vm["version"].as<std::string>()));
    }
    catch(...)
    {
      gzerr << "Invalid version number["
        <<this->vm["version"].as<std::string>() << "]\n";
      return false;
    }
  }

  if (!sdf::init(sdf))
  {
    std::cerr << "ERROR: SDF parsing the xml failed" << std::endl;
    return -1;
  }

  if (this->vm.count("check"))
  {
    boost::filesystem::path path = this->vm["check"].as<std::string>();

    if (!boost::filesystem::exists(path))
      std::cerr << "Error: File doesn't exist[" << path.string() << "]\n";

    if (!sdf::readFile(path.string(), sdf))
    {
      std::cerr << "Error: SDF parsing the xml failed\n";
      return -1;
    }

    std::cout << "Check complete\n";
  }
  else if (this->vm.count("describe"))
  {
    sdf->PrintDescription();
  }
  else if (this->vm.count("doc"))
  {
    sdf->PrintDoc();
  }
  else if (this->vm.count("convert"))
  {
    boost::filesystem::path path = this->vm["convert"].as<std::string>();

    if (!boost::filesystem::exists(path))
      std::cerr << "Error: File doesn't exist[" << path.string() << "]\n";

    if (sdf::convertFile(path.string(), sdf::SDF::Version(), sdf))
    {
      sdf->Write(path.string());
      std::cout << "Success\n";
    }
    else
    {
      std::cerr << "Unable to convert file[" << path.string() << "]\n";
      return false;
    }
  }
  else if (this->vm.count("print"))
  {
    boost::filesystem::path path = this->vm["print"].as<std::string>();

    if (!boost::filesystem::exists(path))
      std::cerr << "Error: File doesn't exist[" << path.string() << "]\n";

    if (!sdf::readFile(path.string(), sdf))
    {
      std::cerr << "Error: SDF parsing the xml failed\n";
      return false;
    }
    sdf->PrintValues();
  }
  else
  {
    this->Help();
  }

  return true;
}

/////////////////////////////////////////////////
HelpCommand::HelpCommand()
  : Command("help",
      "Outputs information about a command")
{
  // Options that are visible to the user through help.
  // this->visibleOptions.add_options()
  //  ("option,o", po::value<std::string>(), "Show the command options.");
}

/////////////////////////////////////////////////
void HelpCommand::HelpDetailed()
{
  std::cerr <<
    "\tOutput information about a gz command.\n"
    << std::endl;
}

/////////////////////////////////////////////////
bool HelpCommand::TransportRequired()
{
  return false;
}

/////////////////////////////////////////////////
bool HelpCommand::RunImpl()
{
  std::string option;
  if (vm.count("pass") && !vm["pass"].as<std::vector<std::string> >().empty())
    option = vm["pass"].as<std::vector<std::string> >()[0];

  this->Help(option);

  return true;
}

/////////////////////////////////////////////////
void HelpCommand::Help(const std::string &_command)
{
  std::cerr << "This tool modifies various aspects of a "
    << "running Gazebo simulation.\n\n";

  if (_command.empty() || g_commandMap.find(_command) == g_commandMap.end())
  {
    std::cerr << "  Usage:  gz <command>\n\n"
      << "List of commands:\n\n";

    std::cerr << "  " << std::left << std::setw(10) << std::setfill(' ')
      << "help";
    std::cerr << "Print this help text.\n";

    for (std::map<std::string, Command*>::iterator iter = g_commandMap.begin();
        iter != g_commandMap.end(); ++iter)
    {
      std::cerr << "  " << std::left << std::setw(10) << std::setfill(' ')
        << iter->first;
      std::cerr << iter->second->GetBrief() << "\n";
    }

    std::cerr << "\n\n";
    std::cerr << "Use \"gz help <command>\" to print help for a "
      "command.\n";
  }
  else if (g_commandMap.find(_command) != g_commandMap.end())
    g_commandMap[_command]->Help();
}

/////////////////////////////////////////////////
DebugCommand::DebugCommand()
  : Command("debug",
      "Returns completion list for a command. Used for bash completion.")
{
  // Options that are visible to the user through help.
  this->visibleOptions.add_options()
    ("option,o", po::value<std::string>(), "Show the command options.");
}

/////////////////////////////////////////////////
void DebugCommand::HelpDetailed()
{
  std::cerr <<
    "\tUsed primarily for bash completion, this tool\n"
    "\treturn the completion list for a given command.\n"
    << std::endl;
}

/////////////////////////////////////////////////
bool DebugCommand::TransportRequired()
{
  return false;
}

/////////////////////////////////////////////////
bool DebugCommand::RunImpl()
{
  if (this->vm.count("option") <= 0)
  {
    for (std::map<std::string, Command*>::iterator iter = g_commandMap.begin();
        iter != g_commandMap.end(); ++iter)
    {
      std::cout << iter->first << "\n";
    }
  }
  else
  {
    std::map<std::string, Command *>::iterator iter =
      g_commandMap.find(this->vm["option"].as<std::string>());
    if (iter != g_commandMap.end())
      iter->second->ListOptions();
  }

  return true;
}

//////////////////////////////////////////////////
void SignalHandler(int /*dummy*/)
{
  Command::Signal();
  return;
}

/////////////////////////////////////////////////
int main(int argc, char **argv)
{
  gazebo::common::Console::SetQuiet(true);

  // Hidden options
  po::options_description hiddenOptions("hidden options");
  hiddenOptions.add_options()
    ("command", po::value<std::string>(), "Command")
    ("pass", po::value<std::vector<std::string> >(), "pass through");

  // Both the hidden and visible options
  po::options_description allOptions("all options");
  allOptions.add(hiddenOptions);

  // The command and file options are positional
  po::positional_options_description positional;
  positional.add("command", 1).add("pass", -1);

  po::variables_map vm;

  try
  {
    po::store(
        po::command_line_parser(argc, argv).options(allOptions).positional(
          positional).allow_unregistered().run(), vm);

    po::notify(vm);
  }
  catch(boost::exception &_e)
  {
    std::cerr << "Invalid arguments\n";
    return -1;
  }

  if (signal(SIGINT, SignalHandler) == SIG_ERR)
  {
    std::cerr << "signal(2) failed while setting up for SIGINT" << std::endl;
    return -1;
  }

  g_commandMap["camera"] = new CameraCommand();
  g_commandMap["help"] = new HelpCommand();
  g_commandMap["joint"] = new JointCommand();
  g_commandMap["marker"] = new MarkerCommand();
  g_commandMap["model"] = new ModelCommand();
  g_commandMap["world"] = new WorldCommand();
  g_commandMap["physics"] = new PhysicsCommand();
  g_commandMap["stats"] = new StatsCommand();
  g_commandMap["topic"] = new TopicCommand();
  g_commandMap["log"] = new LogCommand();
  g_commandMap["sdf"] = new SDFCommand();
  g_commandMap["debug"] = new DebugCommand();

  // Get the command name
  std::string command =
    vm.count("command") ? vm["command"].as<std::string>() : "";

  std::map<std::string, Command *>::iterator iter =
    g_commandMap.find(command);

  int result = 0;

  if (iter != g_commandMap.end())
  {
    g_commandMap[command]->Run(argc, argv);
  }
  else
  {
    g_commandMap["help"]->Run(argc, argv);
    result = -1;
  }

  for (iter = g_commandMap.begin(); iter != g_commandMap.end(); ++iter)
    delete iter->second;

  return result;
}