pxmlw6n2f/Gazebo_Distributed_TCP/gazebo/gui/model/ModelData.cc

/*
 * Copyright (C) 2015 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.
 *
 */

#ifdef _WIN32
  // Ensure that Winsock2.h is included before Windows.h, which can get
  // pulled in by anybody (e.g., Boost).
  #include <Winsock2.h>
#endif

#include <boost/thread/recursive_mutex.hpp>
#include <ignition/math/MassMatrix3.hh>

#include "gazebo/rendering/LinkFrameVisual.hh"
#include "gazebo/rendering/Material.hh"
#include "gazebo/rendering/Scene.hh"
#include "gazebo/rendering/ogre_gazebo.h"

#include "gazebo/gui/model/LinkInspector.hh"
#include "gazebo/gui/model/ModelPluginInspector.hh"
#include "gazebo/gui/model/VisualConfig.hh"
#include "gazebo/gui/model/LinkConfig.hh"
#include "gazebo/gui/model/CollisionConfig.hh"

#include "gazebo/gui/GuiIface.hh"
#include "gazebo/gui/model/ModelData.hh"

using namespace gazebo;
using namespace gui;

/////////////////////////////////////////////////
std::string ModelData::GetTemplateSDFString()
{
  std::ostringstream newModelStr;
  newModelStr << "<sdf version ='" << SDF_VERSION << "'>"
    << "<model name='template_model'>"
    << "<pose>0 0 0.0 0 0 0</pose>"
    << "<link name ='link'>"
    <<   "<visual name ='visual'>"
    <<     "<pose>0 0 0.0 0 0 0</pose>"
    <<     "<geometry>"
    <<       "<box>"
    <<         "<size>1.0 1.0 1.0</size>"
    <<       "</box>"
    <<     "</geometry>"
    <<     "<material>"
    <<       "<lighting>true</lighting>"
    <<       "<script>"
    <<         "<uri>file://media/materials/scripts/gazebo.material</uri>"
    <<         "<name>Gazebo/Grey</name>"
    <<       "</script>"
    <<     "</material>"
    <<   "</visual>"
    << "</link>"
    << "<static>true</static>"
    << "</model>"
    << "</sdf>";

  return newModelStr.str();
}

/////////////////////////////////////////////////
double ModelData::GetEditTransparency()
{
  return 0.4;
}

/////////////////////////////////////////////////
void ModelData::UpdateRenderGroup(rendering::VisualPtr _visual)
{
  // fix for transparency alpha compositing
  if (_visual->GetSceneNode()->numAttachedObjects() <= 0)
    return;

  Ogre::MovableObject *obj = _visual->GetSceneNode()->
      getAttachedObject(0);
  obj->setRenderQueueGroup(obj->getRenderQueueGroup()+1);
}

/////////////////////////////////////////////////
void NestedModelData::SetName(const std::string &_name)
{
  this->modelSDF->GetAttribute("name")->Set(_name);
}

/////////////////////////////////////////////////
void NestedModelData::SetPose(const ignition::math::Pose3d &_pose)
{
  this->modelSDF->GetElement("pose")->Set(_pose);
}

/////////////////////////////////////////////////
ignition::math::Pose3d NestedModelData::Pose() const
{
  return this->modelSDF->Get<ignition::math::Pose3d>("pose");
}

/////////////////////////////////////////////////
int NestedModelData::Depth() const
{
  if (!this->modelVisual)
    return -1;

  return this->modelVisual->GetDepth();
}

/////////////////////////////////////////////////
LinkData::LinkData()
{
  this->linkSDF.reset(new sdf::Element);
  sdf::initFile("link.sdf", this->linkSDF);

  this->scale = ignition::math::Vector3d::One;
  this->inertiaIxx = 0;
  this->inertiaIyy = 0;
  this->inertiaIzz = 0;
  this->mass = 0;
  this->nested = false;

  this->inspector = new LinkInspector();
  this->inspector->setModal(false);
  connect(this->inspector, SIGNAL(Opened()), this, SLOT(OnInspectorOpened()));
  connect(this->inspector, SIGNAL(Applied()), this, SLOT(OnApply()));
  connect(this->inspector, SIGNAL(Accepted()), this, SLOT(OnAccept()));

  this->connect(this->inspector, SIGNAL(ShowCollisions(const bool)),
      this, SLOT(ShowCollisions(const bool)));

  this->connect(this->inspector, SIGNAL(ShowVisuals(const bool)),
      this, SLOT(ShowVisuals(const bool)));

  this->connect(this->inspector, SIGNAL(ShowLinkFrame(const bool)),
      this, SLOT(ShowLinkFrame(const bool)));

  this->connect(this->inspector->GetVisualConfig(),
      SIGNAL(VisualAdded(const std::string &)),
      this, SLOT(OnAddVisual(const std::string &)));

  this->connect(this->inspector->GetCollisionConfig(),
      SIGNAL(CollisionAdded(const std::string &)),
      this, SLOT(OnAddCollision(const std::string &)));

  this->connect(this->inspector->GetVisualConfig(),
      SIGNAL(VisualRemoved(const std::string &)), this,
      SLOT(OnRemoveVisual(const std::string &)));

  this->connect(this->inspector->GetCollisionConfig(),
      SIGNAL(CollisionRemoved(const std::string &)),
      this, SLOT(OnRemoveCollision(const std::string &)));

  this->connect(this->inspector->GetCollisionConfig(),
      SIGNAL(ShowCollision(const bool, const std::string &)),
      this, SLOT(OnShowCollision(const bool, const std::string &)));

  this->connect(this->inspector->GetVisualConfig(),
      SIGNAL(ShowVisual(const bool, const std::string &)),
      this, SLOT(OnShowVisual(const bool, const std::string &)));

  // note the destructor removes this connection with the assumption that it is
  // the first one in the vector
  this->connections.push_back(event::Events::ConnectPreRender(
      boost::bind(&LinkData::Update, this)));
  this->updateMutex = new boost::recursive_mutex();
}

/////////////////////////////////////////////////
LinkData::~LinkData()
{
  event::Events::DisconnectPreRender(this->connections[0]);
  this->connections.clear();
  delete this->inspector;
  delete this->updateMutex;
}

/////////////////////////////////////////////////
rendering::VisualPtr LinkData::LinkVisual() const
{
  return this->linkVisual;
}

/////////////////////////////////////////////////
void LinkData::SetLinkVisual(rendering::VisualPtr _linkVisual)
{
  this->linkVisual = _linkVisual;

  if (!this->linkFrameVis)
  {
    rendering::LinkFrameVisualPtr vis(new rendering::LinkFrameVisual(
        _linkVisual->GetName() + "_LINK_FRAME_VISUAL__", _linkVisual));
    vis->Load();
    vis->SetVisible(this->showLinkFrame);

    this->linkFrameVis = vis;
  }
}

/////////////////////////////////////////////////
std::string LinkData::GetName() const
{
  return this->linkSDF->Get<std::string>("name");
}

/////////////////////////////////////////////////
void LinkData::SetName(const std::string &_name)
{
  this->linkSDF->GetAttribute("name")->Set(_name);
  this->inspector->SetName(_name);
}

/////////////////////////////////////////////////
ignition::math::Pose3d LinkData::Pose() const
{
  return this->linkSDF->Get<ignition::math::Pose3d>("pose");
}

/////////////////////////////////////////////////
void LinkData::SetPose(const ignition::math::Pose3d &_pose)
{
  this->linkSDF->GetElement("pose")->Set(_pose);

  LinkConfig *linkConfig = this->inspector->GetLinkConfig();
  linkConfig->SetPose(_pose);
}

/////////////////////////////////////////////////
void LinkData::SetScale(const ignition::math::Vector3d &_scale)
{
  VisualConfig *visualConfig = this->inspector->GetVisualConfig();

  ignition::math::Vector3d dScale = _scale / this->scale;
  for (auto const &it : this->visuals)
  {
    std::string name = it.first->GetName();
    std::string linkName = this->linkVisual->GetName();
    std::string leafName =
        name.substr(name.find(linkName)+linkName.size()+2);
    ignition::math::Vector3d visOldSize;
    std::string uri;
    visualConfig->Geometry(leafName,  visOldSize, uri);
    ignition::math::Vector3d visNewSize = it.first->GetGeometrySize();
    visualConfig->SetGeometry(leafName, visNewSize);
  }

  std::map<std::string, ignition::math::Vector3d> colOldSizes;
  std::map<std::string, ignition::math::Vector3d> colNewSizes;
  CollisionConfig *collisionConfig = this->inspector->GetCollisionConfig();
  for (auto const &it : this->collisions)
  {
    std::string name = it.first->GetName();
    std::string linkName = this->linkVisual->GetName();
    std::string leafName =
        name.substr(name.find(linkName)+linkName.size()+2);

    ignition::math::Vector3d colOldSize;
    std::string uri;
    collisionConfig->Geometry(leafName,  colOldSize, uri);
    ignition::math::Vector3d colNewSize = it.first->GetGeometrySize();
    collisionConfig->SetGeometry(leafName, colNewSize);
    colOldSizes[name] = colOldSize;
    colNewSizes[name] = colNewSize;
  }

  if (this->collisions.empty())
    return;

  // update link inertial values - assume uniform density
  LinkConfig *linkConfig = this->inspector->GetLinkConfig();
  sdf::ElementPtr inertialElem = this->linkSDF->GetElement("inertial");

  // update mass
  // density = mass / volume
  // assume fixed density and scale mass based on volume changes.
  double volumeRatio = 1;
  double newVol = 0;
  double oldVol = 0;
  ignition::math::Vector3d newSize;
  for (auto const &it : this->collisions)
  {
    ignition::math::Vector3d oldSize = colOldSizes[it.first->GetName()];
    newSize = colNewSizes[it.first->GetName()];
    std::string geomStr = it.first->GetGeometryType();
    if (geomStr == "sphere")
    {
      double r = oldSize.X() * 0.5;
      double r3 = r*r*r;
      double newR = newSize.X() * 0.5;
      double newR3 = newR*newR*newR;
      // sphere volume: 4/3 * PI * r^3
      oldVol += 4.0 / 3.0 * IGN_PI * r3;
      newVol += 4.0 / 3.0 * IGN_PI * newR3;
    }
    else if (geomStr == "cylinder")
    {
      double r = oldSize.X() * 0.5;
      double r2 = r*r;
      double newR = newSize.X() * 0.5;
      double newR2 = newR*newR;
      // cylinder volume: PI * r^2 * height
      oldVol += IGN_PI * r2 * oldSize.Z();
      newVol += IGN_PI * newR2 * newSize.Z();
    }
    else
    {
      // box, mesh, and other geometry types - use bounding box
      oldVol += oldSize.X() * oldSize.Y() * oldSize.Z();
      newVol += newSize.X() * newSize.Y() * newSize.Z();
    }
  }

  if (oldVol < 1e-10)
  {
    gzerr << "Volume is too small to compute accurate inertial values"
        << std::endl;
    return;
  }

  volumeRatio = newVol / oldVol;

  // set new mass
  double newMass = this->mass * volumeRatio;
  this->mass = newMass;
  linkConfig->SetMass(newMass);

  // scale the inertia values
  // 1) compute inertia size based on current inertia matrix and geometry
  // 2) apply scale to inertia size
  // 3) compute new inertia values based on new size

  // get current inertia values
  double ixx = this->inertiaIxx;
  double iyy = this->inertiaIyy;
  double izz = this->inertiaIzz;

  double newIxx = ixx;
  double newIyy = iyy;
  double newIzz = izz;
  ignition::math::MassMatrix3d m;

  // we can compute better estimates of inertia values if the link only has
  // one collision made up of a simple shape
  // otherwise assume box geom
  bool boxInertia = true;
  if (this->collisions.size() == 1u)
  {
    auto const &it = this->collisions.begin();
    std::string geomStr = it->first->GetGeometryType();
    if (geomStr == "sphere")
    {
      boxInertia = false;
      double r = newSize.X() * 0.5;
      // Get inertia properties of uniform sphere
      if (!m.SetFromSphere(newMass, r))
      {
        gzerr << "Error computing inertia, not re-scaling" << std::endl;
      }
    }
    else if (geomStr == "cylinder")
    {
      boxInertia = false;
      double newL = newSize.Z();
      double newR = newSize.X() * 0.5;
      // Get inertia properties of uniform cylinder
      if (!m.SetFromCylinderZ(newMass, newL, newR))
      {
        gzerr << "Error computing inertia, not re-scaling" << std::endl;
      }
    }
  }

  if (boxInertia)
  {
    // Get inertia properties of uniform box
    if (!m.SetFromBox(newMass, newSize))
    {
      gzerr << "Error computing inertia, not re-scaling" << std::endl;
    }
  }

  if (m.IsValid())
  {
    newIxx = m.IXX();
    newIyy = m.IYY();
    newIzz = m.IZZ();
  }

  // update inspector inertia
  linkConfig->SetInertiaMatrix(newIxx, newIyy, newIzz, 0, 0, 0);

  // update local inertal variables
  this->inertiaIxx = newIxx;
  this->inertiaIyy = newIyy;
  this->inertiaIzz = newIzz;

  // update sdf
  sdf::ElementPtr inertiaElem = inertialElem->GetElement("inertia");
  sdf::ElementPtr ixxElem = inertiaElem->GetElement("ixx");
  sdf::ElementPtr iyyElem = inertiaElem->GetElement("iyy");
  sdf::ElementPtr izzElem = inertiaElem->GetElement("izz");
  ixxElem->Set(newIxx);
  iyyElem->Set(newIyy);
  izzElem->Set(newIzz);

  sdf::ElementPtr massElem = inertialElem->GetElement("mass");
  massElem->Set(newMass);

  sdf::ElementPtr inertialPoseElem = inertialElem->GetElement("pose");
  ignition::math::Pose3d newPose =
      inertialPoseElem->Get<ignition::math::Pose3d>();
  newPose.Pos() *= dScale;

  inertialPoseElem->Set(newPose);
  linkConfig->SetInertialPose(newPose);

  this->scale = _scale;
}

/////////////////////////////////////////////////
ignition::math::Vector3d LinkData::Scale() const
{
  return this->scale;
}

/////////////////////////////////////////////////
void LinkData::Load(sdf::ElementPtr _sdf)
{
  LinkConfig *linkConfig = this->inspector->GetLinkConfig();

  this->SetName(_sdf->Get<std::string>("name"));
  this->SetPose(_sdf->Get<ignition::math::Pose3d>("pose"));

  msgs::LinkPtr linkMsgPtr(new msgs::Link);
  if (_sdf->HasElement("inertial"))
  {
    sdf::ElementPtr inertialElem = _sdf->GetElement("inertial");
    this->linkSDF->GetElement("inertial")->Copy(inertialElem);

    msgs::Inertial *inertialMsg = linkMsgPtr->mutable_inertial();

    if (inertialElem->HasElement("mass"))
    {
      this->mass = inertialElem->Get<double>("mass");
      inertialMsg->set_mass(this->mass);
    }

    if (inertialElem->HasElement("pose"))
    {
      ignition::math::Pose3d inertialPose =
        inertialElem->Get<ignition::math::Pose3d>("pose");
      msgs::Set(inertialMsg->mutable_pose(), inertialPose);
    }

    if (inertialElem->HasElement("inertia"))
    {
      sdf::ElementPtr inertiaElem = inertialElem->GetElement("inertia");
      this->inertiaIxx = inertiaElem->Get<double>("ixx");
      this->inertiaIyy = inertiaElem->Get<double>("iyy");
      this->inertiaIzz = inertiaElem->Get<double>("izz");
      inertialMsg->set_ixx(this->inertiaIxx);
      inertialMsg->set_iyy(this->inertiaIyy);
      inertialMsg->set_izz(this->inertiaIzz);
      inertialMsg->set_ixy(inertiaElem->Get<double>("ixy"));
      inertialMsg->set_ixz(inertiaElem->Get<double>("ixz"));
      inertialMsg->set_iyz(inertiaElem->Get<double>("iyz"));
    }
  }
  if (_sdf->HasElement("self_collide"))
  {
    sdf::ElementPtr selfCollideSDF = _sdf->GetElement("self_collide");
    linkMsgPtr->set_self_collide(selfCollideSDF->Get<bool>(""));
    this->linkSDF->InsertElement(selfCollideSDF->Clone());
  }
  if (_sdf->HasElement("kinematic"))
  {
    sdf::ElementPtr kinematicSDF = _sdf->GetElement("kinematic");
    linkMsgPtr->set_kinematic(kinematicSDF->Get<bool>());
    this->linkSDF->InsertElement(kinematicSDF->Clone());
  }
  if (_sdf->HasElement("must_be_base_link"))
  {
    sdf::ElementPtr baseLinkSDF = _sdf->GetElement("must_be_base_link");
    // TODO link.proto is missing the must_be_base_link field.
    // linkMsgPtr->set_must_be_base_link(baseLinkSDF->Get<bool>());
    this->linkSDF->InsertElement(baseLinkSDF->Clone());
  }
  if (_sdf->HasElement("velocity_decay"))
  {
    sdf::ElementPtr velocityDecaySDF = _sdf->GetElement("velocity_decay");
    // TODO link.proto is missing the velocity_decay field.
    // linkMsgPtr->set_velocity_decay(velocityDecaySDF->Get<double>());
    this->linkSDF->InsertElement(velocityDecaySDF->Clone());
  }
  linkConfig->Update(linkMsgPtr);

  if (_sdf->HasElement("sensor"))
  {
    sdf::ElementPtr sensorElem = _sdf->GetElement("sensor");
    while (sensorElem)
    {
      this->linkSDF->InsertElement(sensorElem->Clone());
      sensorElem = sensorElem->GetNextElement("sensor");
    }
  }
}

/////////////////////////////////////////////////
void LinkData::OnShowCollision(const bool _show, const std::string &_name)
{
  for (auto col : this->collisions)
  {
    auto leafName = col.first->GetName();
    size_t idx = leafName.rfind("::");
    if (idx != std::string::npos)
      leafName = leafName.substr(idx+2);

    // Only show one collision
    if (leafName == _name)
    {
      col.first->SetVisible(_show);
      return;
    }
  }
}

/////////////////////////////////////////////////
void LinkData::OnShowVisual(const bool _show, const std::string &_name)
{
  for (auto col : this->visuals)
  {
    auto leafName = col.first->GetName();
    size_t idx = leafName.rfind("::");
    if (idx != std::string::npos)
      leafName = leafName.substr(idx+2);

    // Only show one visual
    if (leafName == _name)
    {
      col.first->SetVisible(_show);
      return;
    }
  }
}

/////////////////////////////////////////////////
void LinkData::ShowCollisions(const bool _show)
{
  this->showCollisions = _show;

  // Check inspector button
  this->inspector->SetShowCollisions(_show);

  auto config = this->inspector->GetCollisionConfig();

  for (auto col : this->collisions)
  {
    auto leafName = col.first->GetName();
    size_t idx = leafName.rfind("::");
    if (idx != std::string::npos)
      leafName = leafName.substr(idx+2);

    // Show all collisions and set button checked
    config->SetShowCollision(_show, leafName);
    col.first->SetVisible(_show);
  }
}

/////////////////////////////////////////////////
void LinkData::ShowVisuals(const bool _show)
{
  this->showVisuals = _show;

  // Check inspector button
  this->inspector->SetShowVisuals(_show);

  auto config = this->inspector->GetVisualConfig();

  for (auto col : this->visuals)
  {
    auto leafName = col.first->GetName();
    size_t idx = leafName.rfind("::");
    if (idx != std::string::npos)
      leafName = leafName.substr(idx+2);

    // Show all visuals and set button checked
    config->SetShowVisual(_show, leafName);
    col.first->SetVisible(_show);
  }
}

/////////////////////////////////////////////////
void LinkData::ShowLinkFrame(const bool _show)
{
  this->showLinkFrame = _show;

  // Check inspector button
  this->inspector->SetShowLinkFrame(_show);

  this->linkFrameVis->SetVisible(_show);
}

/////////////////////////////////////////////////
void LinkData::UpdateConfig()
{
  // set new geom size if scale has changed.
  VisualConfig *visualConfig = this->inspector->GetVisualConfig();
  for (auto &it : this->visuals)
  {
    std::string name = it.first->GetName();
    std::string leafName = name;
    size_t idx = name.rfind("::");
    if (idx != std::string::npos)
      leafName = name.substr(idx+2);
    visualConfig->SetGeometry(leafName, it.first->GetGeometrySize(),
        it.first->GetMeshName());

    msgs::Visual *updateMsg = visualConfig->GetData(leafName);
    msgs::Visual visualMsg = it.second;
    updateMsg->clear_scale();
    msgs::Material *matMsg = updateMsg->mutable_material();
    // clear empty colors so they are not used by visual updates
    common::Color emptyColor;
    if (msgs::Convert(matMsg->ambient()) == emptyColor)
      matMsg->clear_ambient();
    if (msgs::Convert(matMsg->diffuse()) == emptyColor)
      matMsg->clear_diffuse();
    if (msgs::Convert(matMsg->specular()) == emptyColor)
      matMsg->clear_specular();
    if (msgs::Convert(matMsg->emissive()) == emptyColor)
      matMsg->clear_emissive();

    if (matMsg->has_diffuse())
      matMsg->mutable_diffuse()->set_a(1.0-updateMsg->transparency());

    visualMsg.CopyFrom(*updateMsg);
    it.second = visualMsg;
  }
  CollisionConfig *collisionConfig = this->inspector->GetCollisionConfig();
  for (auto &colIt : this->collisions)
  {
    std::string name = colIt.first->GetName();
    std::string leafName = name;
    size_t idx = name.rfind("::");
    if (idx != std::string::npos)
      leafName = name.substr(idx+2);
    collisionConfig->SetGeometry(leafName, colIt.first->GetGeometrySize(),
        colIt.first->GetMeshName());

    msgs::Collision *updateMsg = collisionConfig->GetData(leafName);
    msgs::Collision collisionMsg = colIt.second;
    collisionMsg.CopyFrom(*updateMsg);
    colIt.second = collisionMsg;
  }
}

/////////////////////////////////////////////////
void LinkData::AddVisual(rendering::VisualPtr _visual)
{
  VisualConfig *visualConfig = this->inspector->GetVisualConfig();
  msgs::Visual visualMsg = msgs::VisualFromSDF(_visual->GetSDF());

  _visual->SetVisible(this->showVisuals);

  this->visuals[_visual] = visualMsg;

  std::string visName = _visual->GetName();
  std::string leafName = visName;
  size_t idx = visName.rfind("::");
  if (idx != std::string::npos)
    leafName = visName.substr(idx+2);

  visualConfig->AddVisual(leafName, &visualMsg);
  this->linkFrameVis->RecalculateScale();
}

/////////////////////////////////////////////////
void LinkData::AddCollision(rendering::VisualPtr _collisionVis,
    const msgs::Collision *_msg)
{
  CollisionConfig *collisionConfig = this->inspector->GetCollisionConfig();

  sdf::ElementPtr collisionSDF(new sdf::Element);
  sdf::initFile("collision.sdf", collisionSDF);

  std::string visName = _collisionVis->GetName();
  std::string leafName = visName;
  size_t idx = visName.rfind("::");
  if (idx != std::string::npos)
    leafName = visName.substr(idx+2);

  msgs::Collision collisionMsg;
  // Use input message
  if (_msg)
  {
    collisionMsg = *_msg;
  }
  // Get data from input visual
  else
  {
    msgs::Visual visualMsg = msgs::VisualFromSDF(_collisionVis->GetSDF());
    collisionMsg.set_name(leafName);
    msgs::Geometry *geomMsg = collisionMsg.mutable_geometry();
    geomMsg->CopyFrom(visualMsg.geometry());
    msgs::Pose *poseMsg = collisionMsg.mutable_pose();
    poseMsg->CopyFrom(visualMsg.pose());
  }

  _collisionVis->SetVisible(this->showCollisions);

  this->collisions[_collisionVis] = collisionMsg;
  collisionConfig->AddCollision(leafName, &collisionMsg);

  this->linkFrameVis->RecalculateScale();
}

/////////////////////////////////////////////////
LinkData *LinkData::Clone(const std::string &_newName)
{
  LinkData *cloneLink = new LinkData();

  cloneLink->Load(this->linkSDF);
  cloneLink->SetName(_newName);

  std::string linkVisualName = this->linkVisual->GetName();
  std::string cloneVisName = _newName;
  size_t linkIdx = linkVisualName.find("::");
  if (linkIdx != std::string::npos)
    cloneVisName = linkVisualName.substr(0, linkIdx+2) + _newName;

  // clone linkVisual;
  rendering::VisualPtr linkVis(new rendering::Visual(cloneVisName,
      this->linkVisual->GetParent()));
  linkVis->Load();

  cloneLink->SetLinkVisual(linkVis);

  for (auto &visIt : this->visuals)
  {
    std::string newVisName = visIt.first->GetName();
    size_t idx = newVisName.rfind("::");
    std::string leafName = newVisName.substr(idx+2);
    if (idx != std::string::npos)
      newVisName = cloneVisName + "::" + leafName;
    else
      newVisName = cloneVisName + "::" + newVisName;

    rendering::VisualPtr cloneVis =
        visIt.first->Clone(newVisName, cloneLink->LinkVisual());

    // store the leaf name in sdf not the full scoped name
    cloneVis->GetSDF()->GetAttribute("name")->Set(leafName);

    // override transparency
    cloneVis->SetTransparency(visIt.second.transparency());
    cloneLink->AddVisual(cloneVis);
    cloneVis->SetTransparency(visIt.second.transparency() *
        (1-ModelData::GetEditTransparency()-0.1)
        + ModelData::GetEditTransparency());
  }

  for (auto &colIt : this->collisions)
  {
    std::string newColName = colIt.first->GetName();
    size_t idx = newColName.rfind("::");
    std::string leafName = newColName.substr(idx+2);
    if (idx != std::string::npos)
      newColName = cloneVisName + "::" + leafName;
    else
      newColName = cloneVisName + "::" + newColName;
    rendering::VisualPtr collisionVis = colIt.first->Clone(newColName,
        cloneLink->LinkVisual());

    // store the leaf name in sdf not the full scoped name
    collisionVis->GetSDF()->GetAttribute("name")->Set(leafName);

    collisionVis->SetTransparency(
       ignition::math::clamp(ModelData::GetEditTransparency() * 2.0, 0.0, 0.8));
    ModelData::UpdateRenderGroup(collisionVis);
    cloneLink->AddCollision(collisionVis);
  }
  return cloneLink;
}

/////////////////////////////////////////////////
void LinkData::OnAccept()
{
  if (this->Apply())
    this->inspector->accept();
}

/////////////////////////////////////////////////
void LinkData::OnApply()
{
  this->Apply();
}

/////////////////////////////////////////////////
void LinkData::OnInspectorOpened()
{
  // Reset backup lists
  for (auto it = this->deletedVisuals.begin();
      it != this->deletedVisuals.end(); ++it)
  {
    this->linkVisual->DetachVisual(it->first);
    this->linkVisual->GetScene()->RemoveVisual(it->first);
  }
  this->deletedVisuals.clear();

  for (auto it = this->deletedCollisions.begin();
      it != this->deletedCollisions.end(); ++it)
  {
    this->linkVisual->DetachVisual(it->first);
    this->linkVisual->GetScene()->RemoveVisual(it->first);
  }
  this->deletedCollisions.clear();
}

/////////////////////////////////////////////////
bool LinkData::Apply()
{
  boost::recursive_mutex::scoped_lock lock(*this->updateMutex);
  LinkConfig *linkConfig = this->inspector->GetLinkConfig();

  msgs::Link *linkMsg = linkConfig->GetData();

  // update link sdf
  this->linkSDF = msgs::LinkToSDF(*linkMsg, this->linkSDF);

  // update internal variables
  msgs::Inertial *inertialMsg = linkMsg->mutable_inertial();
  this->mass = inertialMsg->mass();
  this->inertiaIxx = inertialMsg->ixx();
  this->inertiaIyy = inertialMsg->iyy();
  this->inertiaIzz = inertialMsg->izz();

  // update link visual pose
  this->linkVisual->SetPose(this->Pose());

  std::vector<msgs::Visual *> visualUpdateMsgsTemp;
  std::vector<msgs::Collision *> collisionUpdateMsgsTemp;

  // update visuals
  if (!this->visuals.empty())
  {
    VisualConfig *visualConfig = this->inspector->GetVisualConfig();
    for (auto &it : this->visuals)
    {
      std::string name = it.first->GetName();
      std::string leafName = name;
      size_t idx = name.rfind("::");
      if (idx != std::string::npos)
        leafName = name.substr(idx+2);
      msgs::Visual *updateMsg = visualConfig->GetData(leafName);
      if (updateMsg)
      {
        msgs::Visual visualMsg = it.second;

        // check if the geometry is valid
        msgs::Geometry *geomMsg = updateMsg->mutable_geometry();

        // warnings when changing from/to polyline
        for (auto &vis : this->visuals)
        {
          if (vis.second.name() != updateMsg->name())
            continue;

          if (vis.second.mutable_geometry()->type() != geomMsg->type())
          {
            // Changing from polyline, give option to cancel
            if (vis.second.mutable_geometry()->type() ==
                msgs::Geometry::POLYLINE)
            {
              std::string msg =
                  "Once you change the geometry, you can't go "
                  "back to polyline.\n\n"
                  "Do you wish to continue?\n";

              QMessageBox msgBox(QMessageBox::Warning,
                  QString("Changing polyline geometry"), QString(msg.c_str()));

              QPushButton *cancelButton =
                  msgBox.addButton("Cancel", QMessageBox::RejectRole);
              QPushButton *saveButton = msgBox.addButton("Ok",
                  QMessageBox::AcceptRole);
              msgBox.setDefaultButton(saveButton);
              msgBox.setEscapeButton(cancelButton);
              msgBox.exec();
              if (msgBox.clickedButton() != saveButton)
                return false;
            }
            // Changing to polyline: not allowed
            else if (geomMsg->type() == msgs::Geometry::POLYLINE)
            {
              std::string msg =
                  "It's not possible to change into polyline.\n"
                  "Please select another geometry type for ["
                  + leafName + "].";

              QMessageBox::warning(linkConfig, QString(
                  "Invalid geometry conversion"), QString(msg.c_str()),
                  QMessageBox::Ok, QMessageBox::Ok);
              return false;
            }
          }
        }

        if (geomMsg->type() == msgs::Geometry::MESH)
        {
          msgs::MeshGeom *meshGeom = geomMsg->mutable_mesh();
          QFileInfo info(QString::fromStdString(meshGeom->filename()));

          std::string suffix;
          if (info.isFile())
            suffix = info.completeSuffix().toLower().toStdString();

          if (!info.isFile() || (suffix != "dae" && suffix != "stl" &&
              suffix != "obj"))
          {
            std::string msg = "\"" + meshGeom->filename() +
                "\" is not a valid mesh file.\nPlease select another file for ["
                + leafName + "].";

            QMessageBox::warning(linkConfig, QString("Invalid Mesh File"),
                QString(msg.c_str()), QMessageBox::Ok, QMessageBox::Ok);
            return false;
          }
        }

        // update the visualMsg that will be used to generate the sdf.
        updateMsg->clear_scale();
        msgs::Material *matMsg = updateMsg->mutable_material();
        msgs::Material::Script *scriptMsg = matMsg->mutable_script();

        common::Color emptyColor;
        common::Color matAmbient;
        common::Color matDiffuse;
        common::Color matSpecular;
        common::Color matEmissive;
        rendering::Material::GetMaterialAsColor(scriptMsg->name(), matAmbient,
            matDiffuse, matSpecular, matEmissive);

        common::Color ambient = msgs::Convert(matMsg->ambient());
        common::Color diffuse = msgs::Convert(matMsg->diffuse());
        common::Color specular = msgs::Convert(matMsg->specular());
        common::Color emissive = msgs::Convert(matMsg->emissive());

        if (ambient == emptyColor)
        {
          matMsg->clear_ambient();
          ambient = matAmbient;
        }
        if (diffuse == emptyColor)
        {
          matMsg->clear_diffuse();
          diffuse = matDiffuse;
        }
        if (specular == emptyColor)
        {
          matMsg->clear_specular();
          specular = matSpecular;
        }
        if (emissive == emptyColor)
        {
          matMsg->clear_emissive();
          emissive = matEmissive;
        }

        visualConfig->SetMaterial(leafName, scriptMsg->name(), ambient,
            diffuse, specular, emissive);

        visualMsg.CopyFrom(*updateMsg);
        it.second = visualMsg;

        visualUpdateMsgsTemp.push_back(updateMsg);
      }
    }
  }

  // update collisions
  if (!this->collisions.empty())
  {
    CollisionConfig *collisionConfig =
        this->inspector->GetCollisionConfig();
    for (auto &it : this->collisions)
    {
      std::string name = it.first->GetName();
      std::string leafName = name;
      size_t idx = name.rfind("::");
      if (idx != std::string::npos)
        leafName = name.substr(idx+2);
      msgs::Collision *updateMsg = collisionConfig->GetData(leafName);
      if (updateMsg)
      {
        msgs::Collision collisionMsg = it.second;

        // check if the geometry is valid
        msgs::Geometry *geomMsg = updateMsg->mutable_geometry();

        // warnings when changing from/to polyline
        for (auto &col : this->collisions)
        {
          if (col.second.name() != updateMsg->name())
            continue;

          if (col.second.mutable_geometry()->type() != geomMsg->type())
          {
            // Changing from polyline, give option to cancel
            if (col.second.mutable_geometry()->type() ==
                msgs::Geometry::POLYLINE)
            {
              std::string msg =
                  "Once you change the geometry, you can't go "
                  "back to polyline.\n\n"
                  "Do you wish to continue?\n";

              QMessageBox msgBox(QMessageBox::Warning,
                  QString("Changing polyline geometry"), QString(msg.c_str()));

              QPushButton *cancelButton =
                  msgBox.addButton("Cancel", QMessageBox::RejectRole);
              QPushButton *saveButton = msgBox.addButton("Ok",
                  QMessageBox::AcceptRole);
              msgBox.setDefaultButton(saveButton);
              msgBox.setEscapeButton(cancelButton);
              msgBox.exec();
              if (msgBox.clickedButton() != saveButton)
                return false;
            }
            // Changing to polyline: not allowed
            else if (geomMsg->type() == msgs::Geometry::POLYLINE)
            {
              std::string msg =
                  "It's not possible to change into polyline.\n"
                  "Please select another geometry type for ["
                  + leafName + "].";

              QMessageBox::warning(linkConfig, QString(
                  "Invalid geometry conversion"), QString(msg.c_str()),
                  QMessageBox::Ok, QMessageBox::Ok);
              return false;
            }
          }
        }

        if (geomMsg->type() == msgs::Geometry::MESH)
        {
          msgs::MeshGeom *meshGeom = geomMsg->mutable_mesh();
          QFileInfo info(QString::fromStdString(meshGeom->filename()));

          std::string suffix;
          if (info.isFile())
            suffix = info.completeSuffix().toLower().toStdString();

          if (!info.isFile() || (suffix != "dae" && suffix != "stl" &&
              suffix != "obj"))
          {
            std::string msg = "\"" + meshGeom->filename() +
                "\" is not a valid mesh file.\nPlease select another file for ["
                + leafName + "].";

            QMessageBox::warning(linkConfig, QString("Invalid Mesh File"),
                QString(msg.c_str()), QMessageBox::Ok, QMessageBox::Ok);
            return false;
          }
        }

        collisionMsg.CopyFrom(*updateMsg);
        it.second = collisionMsg;

        collisionUpdateMsgsTemp.push_back(updateMsg);
      }
    }
  }

  // Only send update messages if all visuals and collisions are valid
  this->visualUpdateMsgs.insert(this->visualUpdateMsgs.end(),
      visualUpdateMsgsTemp.begin(), visualUpdateMsgsTemp.end());
  this->collisionUpdateMsgs.insert(this->collisionUpdateMsgs.end(),
      collisionUpdateMsgsTemp.begin(), collisionUpdateMsgsTemp.end());
  return true;
}

/////////////////////////////////////////////////
void LinkData::OnAddVisual(const std::string &_name)
{
  // add a visual when the user adds a visual via the inspector's visual tab
  VisualConfig *visualConfig = this->inspector->GetVisualConfig();

  std::ostringstream visualName;
  visualName << this->linkVisual->GetName() << "::" << _name;

  rendering::VisualPtr visVisual;
  msgs::Visual visualMsg;

  // See if this is in the deleted list
  for (auto it = this->deletedVisuals.begin();
      it != this->deletedVisuals.end(); ++it)
  {
    if (it->first->GetName() == visualName.str())
    {
      visVisual = it->first;
      visVisual->SetVisible(true);
      visualMsg = it->second;

      this->deletedVisuals.erase(it);
      break;
    }
  }

  if (!visVisual && !this->visuals.empty())
  {
    // add new visual by cloning last instance
    auto refVisual = this->visuals.rbegin()->first;
    visVisual = refVisual->Clone(visualName.str(), this->linkVisual);

    visualMsg = msgs::VisualFromSDF(visVisual->GetSDF());
    visualMsg.set_transparency(this->visuals[refVisual].transparency());
  }
  else if (!visVisual)
  {
    // create new visual based on sdf template (box)
    sdf::SDFPtr modelTemplateSDF(new sdf::SDF);
    modelTemplateSDF->SetFromString(
        ModelData::GetTemplateSDFString());

    visVisual.reset(new rendering::Visual(visualName.str(),
        this->linkVisual));
    sdf::ElementPtr visualElem =  modelTemplateSDF->Root()
        ->GetElement("model")->GetElement("link")->GetElement("visual");
    visVisual->Load(visualElem);
    visualMsg = msgs::VisualFromSDF(visVisual->GetSDF());
  }

  msgs::VisualPtr visualMsgPtr(new msgs::Visual);
  visualMsgPtr->CopyFrom(visualMsg);
  visualConfig->UpdateVisual(_name, visualMsgPtr);

  visVisual->SetVisible(this->showVisuals);
  visualConfig->SetShowVisual(this->showVisuals, _name);
  this->visuals[visVisual] = visualMsg;
  visVisual->SetTransparency(visualMsg.transparency() *
      (1-ModelData::GetEditTransparency()-0.1)
      + ModelData::GetEditTransparency());
}

/////////////////////////////////////////////////
void LinkData::OnAddCollision(const std::string &_name)
{
  // add a collision when the user adds a collision via the inspector's
  // collision tab
  CollisionConfig *collisionConfig = this->inspector->GetCollisionConfig();

  std::stringstream collisionName;
  collisionName << this->linkVisual->GetName() << "::" << _name;

  rendering::VisualPtr collisionVis;
  msgs::Collision collisionMsg;

  // See if this is in the deleted list
  for (auto it = this->deletedCollisions.begin();
      it != this->deletedCollisions.end(); ++it)
  {
    if (it->first->GetName() == collisionName.str())
    {
      collisionVis = it->first;
      collisionVis->SetVisible(true);
      collisionMsg = it->second;

      this->deletedCollisions.erase(it);
      break;
    }
  }

  if (!collisionVis)
  {
    if (!this->collisions.empty())
    {
      // add new collision by cloning last instance
      collisionVis = this->collisions.rbegin()->first->Clone(
          collisionName.str(), this->linkVisual);
    }
    else
    {
      // create new collision based on sdf template (box)
      sdf::SDFPtr modelTemplateSDF(new sdf::SDF);
      modelTemplateSDF->SetFromString(
          ModelData::GetTemplateSDFString());

      collisionVis.reset(new rendering::Visual(collisionName.str(),
          this->linkVisual));
      sdf::ElementPtr collisionElem =  modelTemplateSDF->Root()
          ->GetElement("model")->GetElement("link")->GetElement("visual");
      collisionVis->Load(collisionElem);
      collisionVis->SetMaterial("Gazebo/Orange");
    }

    msgs::Visual visualMsg = msgs::VisualFromSDF(collisionVis->GetSDF());
    collisionMsg.set_name(_name);
    msgs::Geometry *geomMsg = collisionMsg.mutable_geometry();
    geomMsg->CopyFrom(visualMsg.geometry());
  }

  msgs::CollisionPtr collisionMsgPtr(new msgs::Collision);
  collisionMsgPtr->CopyFrom(collisionMsg);
  collisionConfig->UpdateCollision(_name, collisionMsgPtr);

  collisionVis->SetVisible(this->showCollisions);
  collisionConfig->SetShowCollision(this->showCollisions, _name);
  this->collisions[collisionVis] = collisionMsg;

  collisionVis->SetTransparency(
      ignition::math::clamp(ModelData::GetEditTransparency() * 2.0, 0.0, 0.8));
  ModelData::UpdateRenderGroup(collisionVis);
}

/////////////////////////////////////////////////
void LinkData::OnRemoveVisual(const std::string &_name)
{
  // find and remove visual when the user removes it in the
  // inspector's visual tab
  std::ostringstream name;
  name << this->linkVisual->GetName() << "::" << _name;
  std::string visualName = name.str();

  for (auto it = this->visuals.begin(); it != this->visuals.end(); ++it)
  {
    if (visualName == it->first->GetName())
    {
      it->first->SetVisible(false);

      this->deletedVisuals[it->first] = it->second;
      this->visuals.erase(it);
      break;
    }
  }
}

/////////////////////////////////////////////////
void LinkData::OnRemoveCollision(const std::string &_name)
{
  // find and remove collision visual when the user removes it in the
  // inspector's collision tab
  std::ostringstream name;
  name << this->linkVisual->GetName() << "::" << _name;
  std::string collisionName = name.str();

  for (auto it = this->collisions.begin(); it != this->collisions.end(); ++it)
  {
    if (collisionName == it->first->GetName())
    {
      it->first->SetVisible(false);

      this->deletedCollisions[it->first] = it->second;
      this->collisions.erase(it);
      break;
    }
  }
}

/////////////////////////////////////////////////
void LinkData::Update()
{
  boost::recursive_mutex::scoped_lock lock(*this->updateMutex);

  while (!this->visualUpdateMsgs.empty())
  {
    boost::shared_ptr<gazebo::msgs::Visual> updateMsgPtr;
    updateMsgPtr.reset(new msgs::Visual);
    updateMsgPtr->CopyFrom(*this->visualUpdateMsgs.front());

    this->visualUpdateMsgs.erase(this->visualUpdateMsgs.begin());
    for (auto &it : this->visuals)
    {
      if (it.second.name() == updateMsgPtr->name())
      {
        // make visual semi-transparent here
        // but generated sdf will use the correct transparency value
        it.first->UpdateFromMsg(updateMsgPtr);
        it.first->SetTransparency(updateMsgPtr->transparency() *
            (1-ModelData::GetEditTransparency()-0.1)
            + ModelData::GetEditTransparency());
        break;
      }
    }
  }

  while (!this->collisionUpdateMsgs.empty())
  {
    msgs::Collision collisionMsg = *this->collisionUpdateMsgs.front();
    this->collisionUpdateMsgs.erase(this->collisionUpdateMsgs.begin());
    for (auto &it : this->collisions)
    {
      if (it.second.name() == collisionMsg.name())
      {
        msgs::Visual collisionVisMsg;
        msgs::Geometry *geomMsg = collisionVisMsg.mutable_geometry();
        geomMsg->CopyFrom(collisionMsg.geometry());
        msgs::Pose *poseMsg = collisionVisMsg.mutable_pose();
        poseMsg->CopyFrom(collisionMsg.pose());

        boost::shared_ptr<gazebo::msgs::Visual> updateMsgPtr;
        updateMsgPtr.reset(new msgs::Visual);
        updateMsgPtr->CopyFrom(collisionVisMsg);
        std::string origGeomType = it.first->GetGeometryType();
        it.first->UpdateFromMsg(updateMsgPtr);

        // fix for transparency alpha compositing
        if (it.first->GetGeometryType() != origGeomType)
        {
          Ogre::MovableObject *colObj = it.first->GetSceneNode()->
              getAttachedObject(0);
          colObj->setRenderQueueGroup(colObj->getRenderQueueGroup()+1);
        }
        break;
      }
    }
  }
}

/////////////////////////////////////////////////
ModelPluginData::ModelPluginData()
{
  // Initialize SDF
  this->modelPluginSDF.reset(new sdf::Element);
  sdf::initFile("plugin.sdf", this->modelPluginSDF);

  // Inspector
  this->inspector = new ModelPluginInspector();
}

/////////////////////////////////////////////////
ModelPluginData::~ModelPluginData()
{
  delete this->inspector;
}

/////////////////////////////////////////////////
void ModelPluginData::Load(sdf::ElementPtr _pluginElem)
{
  this->modelPluginSDF = _pluginElem;

  // Convert SDF to msg
  msgs::Plugin pluginMsg = msgs::PluginFromSDF(_pluginElem);
  msgs::PluginPtr pluginPtr(new msgs::Plugin);
  pluginPtr->CopyFrom(pluginMsg);

  // Update inspector
  this->inspector->Update(pluginPtr);
}