moveit_visual_tools.h

Go to the documentation of this file.

/*********************************************************************
 * Software License Agreement (BSD License)
 *
 *  Copyright (c) 2015, University of Colorado, Boulder
 *  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 Univ of CO, Boulder 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  Dave Coleman
 * \desc    Helper functions for displaying and debugging MoveIt! data in Rviz via published markers
 *          and MoveIt! collision objects. Very useful for debugging complex software
 */

#ifndef MOVEIT_VISUAL_TOOLS_MOVEIT_VISUAL_TOOLS_H
#define MOVEIT_VISUAL_TOOLS_MOVEIT_VISUAL_TOOLS_H

// Rviz Visualization Tool
#include <rviz_visual_tools/rviz_visual_tools.h>

// MoveIt
#include <moveit/planning_scene_monitor/planning_scene_monitor.h>
// #include <moveit/macros/deprecation.h>
#include <rviz_visual_tools/deprecation.h>

// MoveIt Messages
#include <moveit_msgs/Grasp.h>
#include <moveit_msgs/DisplayRobotState.h>
#include <moveit_msgs/WorkspaceParameters.h>

// ROS Messages
#include <trajectory_msgs/JointTrajectory.h>

// C++
#include <map>
#include <string>
#include <vector>

namespace moveit_visual_tools
{
// Default constants
static const std::string ROBOT_DESCRIPTION = "robot_description";  // this is the default used in ROS
static const std::string DISPLAY_PLANNED_PATH_TOPIC =
    "/move_group/display_planned_path";  // this is the default when adding the Rviz plugin
static const std::string DISPLAY_ROBOT_STATE_TOPIC =
    "display_robot_state";                                         // this is the default when adding the Rviz plugin
static const std::string PLANNING_SCENE_TOPIC = "planning_scene";  // this is the default when adding the Rviz plugin

class MoveItVisualTools : public rviz_visual_tools::RvizVisualTools
{
public:
  MoveItVisualTools(const std::string &base_frame, const std::string &marker_topic,
                    planning_scene_monitor::PlanningSceneMonitorPtr planning_scene_monitor);

  MoveItVisualTools(const std::string &base_frame,
                    const std::string &marker_topic = rviz_visual_tools::RVIZ_MARKER_TOPIC,
                    robot_model::RobotModelConstPtr robot_model = robot_model::RobotModelConstPtr());

  void setRobotStateTopic(const std::string &robot_state_topic)
  {
    robot_state_topic_ = robot_state_topic;
  }

  void setPlanningSceneTopic(const std::string &planning_scene_topic)
  {
    planning_scene_topic_ = planning_scene_topic;
  }

  bool loadPlanningSceneMonitor();

  bool processCollisionObjectMsg(const moveit_msgs::CollisionObject &msg,
                                 const rviz_visual_tools::colors &color = rviz_visual_tools::GREEN);

  bool processAttachedCollisionObjectMsg(const moveit_msgs::AttachedCollisionObject &msg);

  bool triggerPlanningSceneUpdate();

  bool loadSharedRobotState();

  moveit::core::RobotStatePtr &getSharedRobotState();

  moveit::core::RobotStatePtr &getRootRobotState()
  {
    return root_robot_state_;
  }

  moveit::core::RobotModelConstPtr getRobotModel();

  RVIZ_VISUAL_TOOLS_DEPRECATED
  bool loadEEMarker(const std::string &ee_group_name)
  {
    return loadEEMarker(robot_model_->getJointModelGroup(ee_group_name));
  }
  bool loadEEMarker(const robot_model::JointModelGroup *ee_jmg);

  void loadTrajectoryPub(const std::string &display_planned_path_topic = DISPLAY_PLANNED_PATH_TOPIC, bool blocking = true);
  void loadRobotStatePub(const std::string &robot_state_topic = "", bool blocking = true);

  void setPlanningSceneMonitor(planning_scene_monitor::PlanningSceneMonitorPtr planning_scene_monitor)
  {
    planning_scene_monitor_ = planning_scene_monitor;
  }

  void setManualSceneUpdating(bool enable_manual = true)
  {
    mannual_trigger_update_ = enable_manual;
  }

  bool publishEEMarkers(const Eigen::Affine3d &pose, const robot_model::JointModelGroup *ee_jmg,
                        const rviz_visual_tools::colors &color = rviz_visual_tools::CLEAR,
                        const std::string &ns = "end_effector")
  {
    return publishEEMarkers(convertPose(pose), ee_jmg, color, ns);
  }
  bool publishEEMarkers(const geometry_msgs::Pose &pose, const robot_model::JointModelGroup *ee_jmg,
                        const rviz_visual_tools::colors &color = rviz_visual_tools::CLEAR,
                        const std::string &ns = "end_effector");

  bool publishGrasps(const std::vector<moveit_msgs::Grasp> &possible_grasps, const robot_model::JointModelGroup *ee_jmg,
                     double animate_speed = 0.1);

  bool publishAnimatedGrasps(const std::vector<moveit_msgs::Grasp> &possible_grasps,
                             const robot_model::JointModelGroup *ee_jmg, double animate_speed = 0.01);

  bool publishAnimatedGrasp(const moveit_msgs::Grasp &grasp, const robot_model::JointModelGroup *ee_jmg,
                            double animate_speed);

  bool publishIKSolutions(const std::vector<trajectory_msgs::JointTrajectoryPoint> &ik_solutions,
                          const robot_model::JointModelGroup *arm_jmg, double display_time = 0.4);

  bool removeAllCollisionObjects();

  bool cleanupCO(const std::string &name);

  bool cleanupACO(const std::string &name);

  bool attachCO(const std::string &name, const std::string &ee_parent_link);

  bool publishCollisionFloor(double z = 0.0, const std::string &plane_name = "Floor",
                             const rviz_visual_tools::colors &color = rviz_visual_tools::GREEN);

  bool publishCollisionBlock(const geometry_msgs::Pose &block_pose, const std::string &block_name, double block_size,
                             const rviz_visual_tools::colors &color = rviz_visual_tools::GREEN);

  RVIZ_VISUAL_TOOLS_DEPRECATED
  bool publishCollisionRectangle(const Eigen::Vector3d &point1, const Eigen::Vector3d &point2, const std::string &name,
                                 const rviz_visual_tools::colors &color)
  {
    return publishCollisionCuboid(convertPoint(point1), convertPoint(point2), name, color);
  }
  RVIZ_VISUAL_TOOLS_DEPRECATED
  bool publishCollisionRectangle(const geometry_msgs::Point &point1, const geometry_msgs::Point &point2,
                                 const std::string &name, const rviz_visual_tools::colors &color)
  {
    return publishCollisionCuboid(convertPoint(point1), convertPoint(point2), name, color);
  }
  bool publishCollisionCuboid(const Eigen::Vector3d &point1, const Eigen::Vector3d &point2, const std::string &name,
                              const rviz_visual_tools::colors &color = rviz_visual_tools::GREEN);
  bool publishCollisionCuboid(const geometry_msgs::Point &point1, const geometry_msgs::Point &point2,
                              const std::string &name,
                              const rviz_visual_tools::colors &color = rviz_visual_tools::GREEN);

  bool publishCollisionCylinder(const geometry_msgs::Point &a, const geometry_msgs::Point &b,
                                const std::string &object_name, double radius,
                                const rviz_visual_tools::colors &color = rviz_visual_tools::GREEN);
  bool publishCollisionCylinder(const Eigen::Vector3d &a, const Eigen::Vector3d &b, const std::string &object_name,
                                double radius, const rviz_visual_tools::colors &color = rviz_visual_tools::GREEN);

  bool publishCollisionCylinder(const Eigen::Affine3d &object_pose, const std::string &object_name, double radius,
                                double height, const rviz_visual_tools::colors &color = rviz_visual_tools::GREEN);
  bool publishCollisionCylinder(const geometry_msgs::Pose &object_pose, const std::string &object_name, double radius,
                                double height, const rviz_visual_tools::colors &color = rviz_visual_tools::GREEN);

  bool publishCollisionMesh(const geometry_msgs::Pose &object_pose, const std::string &object_name,
                            const std::string &mesh_path,
                            const rviz_visual_tools::colors &color = rviz_visual_tools::GREEN);
  bool publishCollisionMesh(const Eigen::Affine3d &object_pose, const std::string &object_name,
                            const std::string &mesh_path,
                            const rviz_visual_tools::colors &color = rviz_visual_tools::GREEN);
  bool publishCollisionMesh(const Eigen::Affine3d &object_pose, const std::string &object_name,
                            const shape_msgs::Mesh &mesh_msg,
                            const rviz_visual_tools::colors &color = rviz_visual_tools::GREEN);
  bool publishCollisionMesh(const geometry_msgs::Pose &object_pose, const std::string &object_name,
                            const shape_msgs::Mesh &mesh_msg,
                            const rviz_visual_tools::colors &color = rviz_visual_tools::GREEN);

  bool publishCollisionGraph(const graph_msgs::GeometryGraph &graph, const std::string &object_name, double radius,
                             const rviz_visual_tools::colors &color = rviz_visual_tools::GREEN);

  void getCollisionWallMsg(double x, double y, double angle, double width, double height, const std::string name,
                           moveit_msgs::CollisionObject &collision_obj);

  RVIZ_VISUAL_TOOLS_DEPRECATED
  bool publishCollisionWall(double x, double y, double angle, double width, const std::string name,
                            const rviz_visual_tools::colors &color = rviz_visual_tools::GREEN);

  bool publishCollisionWall(double x, double y, double angle = 0.0, double width = 2.0, double height = 1.5,
                            const std::string name = "wall",
                            const rviz_visual_tools::colors &color = rviz_visual_tools::GREEN);

  bool publishCollisionTable(double x, double y, double angle, double width, double height, double depth,
                             const std::string name, const rviz_visual_tools::colors &color = rviz_visual_tools::GREEN);

  bool loadCollisionSceneFromFile(const std::string &path);
  bool loadCollisionSceneFromFile(const std::string &path, const Eigen::Affine3d &offset);

  RVIZ_VISUAL_TOOLS_DEPRECATED
  bool publishCollisionTests();

  bool publishWorkspaceParameters(const moveit_msgs::WorkspaceParameters &params);

  bool publishContactPoints(const moveit::core::RobotState &robot_state,
                            const planning_scene::PlanningScene *planning_scene,
                            const rviz_visual_tools::colors &color = rviz_visual_tools::RED);

  bool publishTrajectoryPoint(const trajectory_msgs::JointTrajectoryPoint &trajectory_pt,
                              const std::string &planning_group, double display_time = 0.1);

  bool publishTrajectoryPath(const std::vector<moveit::core::RobotStatePtr> &trajectory,
                             const moveit::core::JointModelGroup *jmg, double speed = 0.01, bool blocking = false);
  bool publishTrajectoryPath(const robot_trajectory::RobotTrajectoryPtr &trajectory, bool blocking = false);
  bool publishTrajectoryPath(const robot_trajectory::RobotTrajectory &trajectory, bool blocking = false);
  bool publishTrajectoryPath(const moveit_msgs::RobotTrajectory &trajectory_msg,
                             const moveit::core::RobotStateConstPtr robot_state, bool blocking = false);

  RVIZ_VISUAL_TOOLS_DEPRECATED
  bool publishTrajectoryLine(const moveit_msgs::RobotTrajectory &trajectory_msg,
                             const moveit::core::LinkModel *ee_parent_link, const robot_model::JointModelGroup *arm_jmg,
                             const rviz_visual_tools::colors &color, bool clear_all_markers)
  {
    // Group together messages
    enableInternalBatchPublishing(true);

    if (clear_all_markers)
      publishMarker(reset_marker_);

    return publishTrajectoryLine(trajectory_msg, ee_parent_link, arm_jmg, color);
  }

  RVIZ_VISUAL_TOOLS_DEPRECATED
  bool publishTrajectoryLine(const robot_trajectory::RobotTrajectoryPtr robot_trajectory,
                             const moveit::core::LinkModel *ee_parent_link, const rviz_visual_tools::colors &color,
                             bool clear_all_markers)
  {
    // Group together messages
    enableInternalBatchPublishing(true);

    if (clear_all_markers)
      publishMarker(reset_marker_);

    return publishTrajectoryLine(robot_trajectory, ee_parent_link, color);
  }

  RVIZ_VISUAL_TOOLS_DEPRECATED
  bool publishTrajectoryLine(const robot_trajectory::RobotTrajectory &robot_trajectory,
                             const moveit::core::LinkModel *ee_parent_link, const rviz_visual_tools::colors &color,
                             bool clear_all_markers)
  {
    // Group together messages
    enableInternalBatchPublishing(true);

    if (clear_all_markers)
      publishMarker(reset_marker_);

    return publishTrajectoryLine(robot_trajectory, ee_parent_link, color);
  }

  bool publishTrajectoryLine(const moveit_msgs::RobotTrajectory &trajectory_msg,
                             const moveit::core::LinkModel *ee_parent_link, const robot_model::JointModelGroup *arm_jmg,
                             const rviz_visual_tools::colors &color);
  bool publishTrajectoryLine(const robot_trajectory::RobotTrajectoryPtr robot_trajectory,
                             const moveit::core::LinkModel *ee_parent_link, const rviz_visual_tools::colors &color);
  bool publishTrajectoryLine(const robot_trajectory::RobotTrajectory &robot_trajectory,
                             const moveit::core::LinkModel *ee_parent_link, const rviz_visual_tools::colors &color);

  bool publishTrajectoryPoints(const std::vector<moveit::core::RobotStatePtr> &robot_state_trajectory,
                               const moveit::core::LinkModel *ee_parent_link,
                               const rviz_visual_tools::colors &color = rviz_visual_tools::YELLOW);

  void enableRobotStateRootOffet(const Eigen::Affine3d &offset);

  void disableRobotStateRootOffet();

  bool publishRobotState(const trajectory_msgs::JointTrajectoryPoint &trajectory_pt,
                         const robot_model::JointModelGroup *jmg,
                         const rviz_visual_tools::colors &color = rviz_visual_tools::DEFAULT);

  bool publishRobotState(const moveit::core::RobotState &robot_state,
                         const rviz_visual_tools::colors &color = rviz_visual_tools::DEFAULT);
  bool publishRobotState(const moveit::core::RobotStatePtr &robot_state,
                         const rviz_visual_tools::colors &color = rviz_visual_tools::DEFAULT);

  bool hideRobot();

  bool applyVirtualJointTransform(moveit::core::RobotState &robot_state, const Eigen::Affine3d &offset);

  void showJointLimits(moveit::core::RobotStatePtr robot_state);

private:
  planning_scene_monitor::PlanningSceneMonitorPtr getPlanningSceneMonitor();

  bool checkForVirtualJoint(const moveit::core::RobotState &robot_state);

protected:
  // ROS publishers
  ros::Publisher pub_display_path_;  // for MoveIt trajectories
  ros::Publisher pub_robot_state_;   // publish a RobotState message

  // Pointer to a Planning Scene Monitor
  planning_scene_monitor::PlanningSceneMonitorPtr planning_scene_monitor_;
  robot_model_loader::RobotModelLoaderPtr rm_loader_;  // so that we can specify our own options

  // End Effector Markers
  std::map<const robot_model::JointModelGroup *, visualization_msgs::MarkerArray> ee_markers_map_;
  std::map<const robot_model::JointModelGroup *, EigenSTL::vector_Affine3d> ee_poses_map_;

  // Cached robot state marker - cache the colored links.
  // Note: Only allows colors provided in rviz_visual_tools to prevent too many robot state messages from being loaded
  // and ensuring efficiency
  std::map<rviz_visual_tools::colors, moveit_msgs::DisplayRobotState> display_robot_msgs_;

  // Pointer to the robot model
  moveit::core::RobotModelConstPtr robot_model_;

  // Note: call loadSharedRobotState() before using this
  moveit::core::RobotStatePtr shared_robot_state_;

  // Note: call loadSharedRobotState() before using this. Use only for hiding the robot
  moveit::core::RobotStatePtr hidden_robot_state_;

  // A robot state whose virtual_joint remains at identity so that getGlobalLinkTransform() isn't tainted
  // Note: call loadSharedRobotState() before using this
  moveit::core::RobotStatePtr root_robot_state_;

  // Optional offset that can be applied to all outgoing/published robot states
  bool robot_state_root_offset_enabled_;
  Eigen::Affine3d robot_state_root_offset_;

  // Prevent the planning scene from always auto-pushing, but rather do it manually
  bool mannual_trigger_update_;

  // ROS topic names to use when starting publishers
  std::string robot_state_topic_;
  std::string planning_scene_topic_;
};  // class

typedef boost::shared_ptr<MoveItVisualTools> MoveItVisualToolsPtr;
typedef boost::shared_ptr<const MoveItVisualTools> MoveItVisualToolsConstPtr;

}  // namespace moveit_visual_tools

#endif  // MOVEIT_VISUAL_TOOLS_MOVEIT_VISUAL_TOOLS_H