frame_marker.h

Go to the documentation of this file.

/*********************************************************************
 * Software License Agreement (BSD License)
 *
 *  Copyright (c) 2013, 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 Willow Garage nor the names of its
 *     contributors may be used to endorse or promote products derived
 *     from this software without specific prior written permission.
 *
 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 *  POSSIBILITY OF SUCH DAMAGE.
 *********************************************************************/

/* Author: Mario Prats */

#ifndef BT_FRAME_MARKER
#define BT_FRAME_MARKER

#include <rviz/display_context.h>
#include <rviz/default_plugin/interactive_markers/interactive_marker.h>

#include <moveit/macros/class_forward.h>
#include <moveit/robot_interaction/robot_interaction.h>
#include <moveit/robot_interaction/interaction_handler.h>
#include <moveit/robot_state/robot_state.h>

#include <QObject>

#include <string>

namespace benchmark_tool
{
class MotionPlanningDisplay;

MOVEIT_CLASS_FORWARD(FrameMarker);

class FrameMarker
{
public:
  visualization_msgs::InteractiveMarker imarker_msg;
  boost::shared_ptr<rviz::InteractiveMarker> imarker;

  FrameMarker(const FrameMarker& frame_marker) : receiver_(NULL), receiver_method_(NULL)
  {
    imarker_msg = frame_marker.imarker_msg;

    visible_x_ = frame_marker.visible_x_;
    visible_y_ = frame_marker.visible_y_;
    visible_z_ = frame_marker.visible_z_;

    parent_node_ = frame_marker.parent_node_;
    context_ = frame_marker.context_;

    selected_ = frame_marker.selected_;
    color_ = frame_marker.color_;

    position_ = frame_marker.imarker->getPosition();
    orientation_ = frame_marker.imarker->getOrientation();

    rebuild();
  }

  FrameMarker(Ogre::SceneNode* parent_node, rviz::DisplayContext* context, const std::string& name,
              const std::string& frame_id, const geometry_msgs::Pose& pose, double scale,
              const std_msgs::ColorRGBA& color, bool is_selected = false, bool visible_x = true, bool visible_y = true,
              bool visible_z = true);

  FrameMarker(Ogre::SceneNode* parent_node, rviz::DisplayContext* context, const std::string& name,
              const std::string& frame_id, const geometry_msgs::Pose& pose, double scale, const float color[4],
              bool is_selected = false, bool visible_x = true, bool visible_y = true, bool visible_z = true);

  virtual void updateMarker(void)
  {
    imarker->processMessage(imarker_msg);
  }

  virtual void hide(void);
  virtual void show(Ogre::SceneNode* scene_node, rviz::DisplayContext* context);

  virtual void showDescription(const std::string& description);
  virtual void hideDescription();

  virtual void setAxisVisibility(bool x, bool y, bool z)
  {
    visible_x_ = x;
    visible_y_ = y;
    visible_z_ = z;
    rebuild();
  }

  virtual void setColor(float r, float g, float b, float a);

  virtual void getPosition(geometry_msgs::Point& position);
  virtual void getOrientation(geometry_msgs::Quaternion& orientation);
  virtual void getPose(Eigen::Affine3d& pose);
  virtual void setPose(Eigen::Affine3d& pose);

  virtual void select(void);
  virtual void unselect(void);

  virtual void setMenu(std::vector<visualization_msgs::MenuEntry> entries)
  {
    menu_entries_ = entries;
    rebuild();
  }

  const std::string& getName()
  {
    return imarker_msg.name;
  }

  void setName(const std::string& name)
  {
    imarker_msg.name = name;
  }

  bool isSelected(void)
  {
    return selected_;
  }

  bool isVisible()
  {
    return static_cast<bool>(imarker);
  }

  void connect(const QObject* receiver, const char* method)
  {
    receiver_ = receiver;
    receiver_method_ = method;
    QObject::connect(imarker.get(), SIGNAL(userFeedback(visualization_msgs::InteractiveMarkerFeedback&)), receiver,
                     method);
  }

  virtual ~FrameMarker()
  {
  }

protected:
  virtual void buildFrom(const std::string& name, const std::string& frame_id, const geometry_msgs::Pose& pose,
                         double scale, const std_msgs::ColorRGBA& color);
  virtual void rebuild();

  std::vector<visualization_msgs::MenuEntry> menu_entries_;

  Ogre::SceneNode* parent_node_;
  rviz::DisplayContext* context_;

  bool selected_;
  bool visible_x_, visible_y_, visible_z_;
  std_msgs::ColorRGBA color_;

  Ogre::Vector3 position_;
  Ogre::Quaternion orientation_;

  const QObject* receiver_;
  const char* receiver_method_;
};

MOVEIT_CLASS_FORWARD(GripperMarker);

class GripperMarker : public FrameMarker
{
public:
  typedef enum { NOT_TESTED, PROCESSING, REACHABLE, NOT_REACHABLE, IN_COLLISION } GripperMarkerState;

  GripperMarker(const GripperMarker& gripper_marker) : FrameMarker(gripper_marker)
  {
    robot_state_ = gripper_marker.robot_state_;
    eef_ = gripper_marker.eef_;
    state_ = gripper_marker.state_;
    display_gripper_mesh_ = gripper_marker.display_gripper_mesh_;

    rebuild();
  }

  GripperMarker(const robot_state::RobotState& robot_state, Ogre::SceneNode* parent_node, rviz::DisplayContext* context,
                const std::string& name, const std::string& frame_id,
                const robot_interaction::RobotInteraction::EndEffector& eef, const geometry_msgs::Pose& pose,
                double scale, const GripperMarkerState& state, bool is_selected = false, bool visible_x = true,
                bool visible_y = true, bool visible_z = true);

  virtual void select(bool display_gripper_mesh = true);
  virtual void unselect(bool display_gripper_mesh = false);

  virtual void setState(const GripperMarkerState& state)
  {
    if (state != state_)
    {
      state_ = state;
      const float* color = stateToColor(state);
      setColor(color[0], color[1], color[2], color[3]);
    }
  }

  const GripperMarkerState& getState()
  {
    return state_;
  }

  void setRobotState(const robot_state::RobotState& robot_state)
  {
    robot_state_ = &robot_state;
  }

  const robot_state::RobotState* getRobotState()
  {
    return robot_state_;
  }

  void setEndEffector(const robot_interaction::RobotInteraction::EndEffector& eef)
  {
    eef_ = eef;
  }

  const robot_interaction::RobotInteraction::EndEffector getEndEffector()
  {
    return eef_;
  }

protected:
  static const float GOAL_NOT_TESTED_COLOR[4];
  static const float GOAL_PROCESSING_COLOR[4];
  static const float GOAL_NOT_REACHABLE_COLOR[4];
  static const float GOAL_REACHABLE_COLOR[4];
  static const float GOAL_COLLISION_COLOR[4];

  virtual void buildFrom(const std::string& name, const std::string& frame_id, const geometry_msgs::Pose& pose,
                         double scale, const std_msgs::ColorRGBA& color);

  const float* stateToColor(const GripperMarkerState& state);

  const robot_state::RobotState* robot_state_;
  robot_interaction::RobotInteraction::EndEffector eef_;

  bool display_gripper_mesh_;
  GripperMarkerState state_;
};

}  // namespace
#endif