urdf.cpp

Go to the documentation of this file.

/*
 * Copyright (c) 2010, 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, Inc. 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 <rve_render_client/urdf.h>
#include <rve_render_client/mesh_instance.h>

#include <urdf/model.h>
#include <rve_common/eigen_conversions.h>

#include <Eigen/Geometry>

namespace rve_render_client
{

URDFPtr createURDF(const urdf::Model& model)
{
  URDFPtr urdf(new URDF(model), destroySceneObject);
  return urdf;
}

URDFLink::URDFLink(const urdf::Model& model, const urdf::Link& link)
{
  name_ = link.name;

  if (link.visual)
  {
    loadInfo(visual_, model, link, *link.visual->geometry, link.visual->origin);
  }
}

URDFLink::~URDFLink()
{
}

void URDFLink::loadInfo(VisualInfo& info, const urdf::Model& model, const urdf::Link& link, const urdf::Geometry& geom, const urdf::Pose& origin)
{
  rve_msgs::Vector3 p;
  p.x = origin.position.x;
  p.y = origin.position.y;
  p.z = origin.position.z;
  rve_msgs::Quaternion q;
  q.x = origin.rotation.x;
  q.y = origin.rotation.y;
  q.z = origin.rotation.z;
  q.w = origin.rotation.w;
  info.offset_position_ = p;
  info.offset_orientation_ = q;

  if (geom.type == urdf::Geometry::MESH)
  {
    const urdf::Mesh& mesh = static_cast<const urdf::Mesh&>(geom);
    if (!mesh.filename.empty())
    {
      MeshInstancePtr m = createMeshInstance(mesh.filename);
      addObject(m);

      // TODO: these functions need to be aggregates
      set_position_ = boost::bind(&MeshInstance::setPosition, m.get(), _1);
      set_orientation_ = boost::bind(&MeshInstance::setOrientation, m.get(), _1);
    }
  }

  // TODO: non-mesh objects
}

void URDFLink::setTransform(const rve_msgs::Vector3& pos, const rve_msgs::Quaternion& orient)
{
  if (set_position_)
  {
    Eigen::Vector3f v(pos.x, pos.y, pos.z);
    Eigen::Quaternionf q(orient.w, orient.x, orient.y, orient.z);
    v += rve_common::msgToEigen(visual_.offset_position_);
    q *= rve_common::msgToEigen(visual_.offset_orientation_);

    set_position_(rve_common::eigenToMsg(v));
    set_orientation_(rve_common::eigenToMsg(q));
  }
}

URDF::URDF(const urdf::Model& model)
{
  load(model);
}

URDF::~URDF()
{
}

void URDF::load(const urdf::Model& model)
{
  clear();

  urdf::Model m;
  m.initParam("robot_description");
  typedef std::vector<boost::shared_ptr<urdf::Link> > V_Link;
  V_Link links;
  m.getLinks(links);
  V_Link::iterator it = links.begin();
  V_Link::iterator end = links.end();
  for (; it != end; ++it)
  {
    const boost::shared_ptr<urdf::Link>& l = *it;
    URDFLinkPtr link(new URDFLink(model, *l));
    addObject(link);
    links_.push_back(link);
  }
}

void URDF::clear()
{
  LinkIterator it = links_.begin();
  LinkIterator end = links_.end();
  for (; it != end; ++it)
  {
    const URDFLinkPtr& link = *it;
    removeObject(link);
  }

  links_.clear();
}

} // namespace rve_render_client