Program Listing for File robot.hpp

↰ Return to documentation for file (/tmp/ws/src/rviz/rviz_default_plugins/include/rviz_default_plugins/robot/robot.hpp)

/*
 * Copyright (c) 2008, Willow Garage, Inc.
 * Copyright (c) 2018, Bosch Software Innovations GmbH.
 * 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.
 */

#ifndef RVIZ_DEFAULT_PLUGINS__ROBOT__ROBOT_HPP_
#define RVIZ_DEFAULT_PLUGINS__ROBOT__ROBOT_HPP_

#include <string>
#include <map>

#include <OgreVector3.h>
#include <OgreQuaternion.h>
#include <OgreAny.h>

#include "urdf/model.h"  // can be replaced later by urdf_model/types.h

#include "rviz_default_plugins/robot/link_updater.hpp"
#include "rviz_default_plugins/visibility_control.hpp"

namespace Ogre
{
class SceneManager;
class Entity;
class SceneNode;
class Quaternion;
class Any;
class RibbonTrail;
class SceneNode;
}  // namespace Ogre

namespace rviz
{
class Object;
class Axes;
}  // namespace rviz

namespace tf
{
class TransformListener;
}

namespace rviz_common
{

class DisplayContext;

namespace properties
{
class Property;
class EnumProperty;
class BoolProperty;
}  // namespace properties
}  // namespace rviz_common

namespace rviz_default_plugins
{
namespace robot
{

class Robot;
class RobotLink;
class RobotJoint;

class RVIZ_DEFAULT_PLUGINS_PUBLIC Robot : public QObject
{
  Q_OBJECT

public:
  Robot(
    Ogre::SceneNode * root_node, rviz_common::DisplayContext * context, const std::string & name,
    rviz_common::properties::Property * parent_property);
  ~Robot() override;

  virtual void load(
    const urdf::ModelInterface & urdf,
    bool visual = true,
    bool collision = true,
    bool mass = true,
    bool inertia = true);

  virtual void clear();

  virtual void update(const LinkUpdater & updater);

  virtual void setVisible(bool visible);

  void setVisualVisible(bool visible);

  void setCollisionVisible(bool visible);

  void setMassVisible(bool visible);

  void setInertiaVisible(bool visible);

  bool isVisible();
  bool isVisualVisible();
  bool isCollisionVisible();

  bool isMassVisible();

  bool isInertiaVisible();

  void setAlpha(float a);
  float getAlpha() {return alpha_;}

  RobotLink * getRootLink() {return root_link_;}
  RobotLink * getLink(const std::string & name);
  RobotJoint * getJoint(const std::string & name);

  typedef std::map<std::string, RobotLink *> M_NameToLink;
  typedef std::map<std::string, RobotJoint *> M_NameToJoint;
  const M_NameToLink & getLinks() const {return links_;}
  const M_NameToJoint & getJoints() const {return joints_;}

  const std::string & getName() {return name_;}

  Ogre::SceneNode * getVisualNode() {return root_visual_node_;}
  Ogre::SceneNode * getCollisionNode() {return root_collision_node_;}
  Ogre::SceneNode * getOtherNode() {return root_other_node_;}
  Ogre::SceneManager * getSceneManager() {return scene_manager_;}
  rviz_common::DisplayContext * getDisplayContext() {return context_;}

  virtual void setPosition(const Ogre::Vector3 & position);
  virtual void setOrientation(const Ogre::Quaternion & orientation);
  virtual void setScale(const Ogre::Vector3 & scale);
  virtual const Ogre::Vector3 & getPosition();
  virtual const Ogre::Quaternion & getOrientation();

  class LinkFactory
  {
public:
    virtual ~LinkFactory() = default;
    virtual RobotLink * createLink(
      Robot * robot,
      const urdf::LinkConstSharedPtr & link,
      const std::string & parent_joint_name,
      bool visual,
      bool collision,
      bool mass,
      bool inertia);
    virtual RobotJoint * createJoint(
      Robot * robot,
      const urdf::JointConstSharedPtr & joint);
  };

  void setLinkFactory(LinkFactory * link_factory);


  enum LinkTreeStyle
  {
    STYLE_LINK_LIST,         // list of all links sorted by link name
    STYLE_DEFAULT = STYLE_LINK_LIST,
    STYLE_JOINT_LIST,        // list of joints sorted by joint name
    STYLE_LINK_TREE,         // tree of links
    STYLE_JOINT_LINK_TREE    // tree of joints with links
  };

  void setLinkTreeStyle(LinkTreeStyle style);

  rviz_common::properties::Property * getLinkTreeProperty() {return link_tree_;}

  // set joint checkboxes and All Links Enabled checkbox based on current link enables.
  void calculateJointCheckboxes();

private Q_SLOTS:
  void changedLinkTreeStyle();
  void changedExpandTree();
  void changedEnableAllLinks();
  void changedExpandLinkDetails();
  void changedExpandJointDetails();

protected:
  void updateLinkVisibilities();

  void unparentLinkProperties();

  // place sub properties under detail (or not)
  void useDetailProperty(bool use_detail);

  void addLinkToLinkTree(
    LinkTreeStyle style, rviz_common::properties::Property * parent,
    RobotLink * link);
  void addJointToLinkTree(
    LinkTreeStyle style, rviz_common::properties::Property * parent,
    RobotJoint * joint);

  // set the value of the EnableAllLinks property without affecting child links/joints.
  void setEnableAllLinksCheckbox(QVariant val);

  void initLinkTreeStyle();
  static bool styleShowLink(LinkTreeStyle style);
  static bool styleShowJoint(LinkTreeStyle style);
  static bool styleIsTree(LinkTreeStyle style);

  Ogre::SceneManager * scene_manager_;

  M_NameToLink links_;
  M_NameToJoint joints_;
  RobotLink * root_link_;

  LinkFactory * link_factory_;

  Ogre::SceneNode * root_visual_node_;
  Ogre::SceneNode * root_collision_node_;
  Ogre::SceneNode * root_other_node_;

  bool visible_;
  bool visual_visible_;
  bool collision_visible_;

  bool mass_visible_;
  bool inertia_visible_;

  rviz_common::DisplayContext * context_;
  rviz_common::properties::Property * link_tree_;
  rviz_common::properties::EnumProperty * link_tree_style_;
  rviz_common::properties::BoolProperty * expand_tree_;
  rviz_common::properties::BoolProperty * expand_link_details_;
  rviz_common::properties::BoolProperty * expand_joint_details_;
  rviz_common::properties::BoolProperty * enable_all_links_;
  std::map<LinkTreeStyle, std::string> style_name_map_;

  bool doing_set_checkbox_;   // used only inside setEnableAllLinksCheckbox()
  bool robot_loaded_;         // true after robot model is loaded.

  // true inside changedEnableAllLinks().  Prevents calculateJointCheckboxes()
  // from recalculating over and over.
  bool in_changed_enable_all_links_;

  std::string name_;
  float alpha_;

private:
  void createLinkProperties(
    const urdf::ModelInterface & urdf,
    bool visual,
    bool collision,
    bool mass,
    bool inertia);
  void createJointProperties(const urdf::ModelInterface & urdf);
  void log_error(
    const RobotLink * link,
    const std::string & visual_or_collision,
    const std::string & position_or_orientation) const;
};

}  // namespace robot
}  // namespace rviz_default_plugins

#endif  // RVIZ_DEFAULT_PLUGINS__ROBOT__ROBOT_HPP_