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>
#include <moveit_msgs/DisplayTrajectory.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 psm);

  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 moveCollisionObject(const Eigen::Isometry3d& pose, const std::string& name,
                           const rviz_visual_tools::colors& color);
  bool moveCollisionObject(const geometry_msgs::Pose& pose, const std::string& name,
                           const rviz_visual_tools::colors& color);

  bool triggerPlanningSceneUpdate();

  bool loadSharedRobotState();

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

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

  moveit::core::RobotModelConstPtr getRobotModel();

  bool loadEEMarker(const robot_model::JointModelGroup* ee_jmg, const std::vector<double>& ee_joint_pos = {});

  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 psm)
  {
    psm_ = psm;
  }

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

  bool publishEEMarkers(const Eigen::Isometry3d& pose, const robot_model::JointModelGroup* ee_jmg,
                        const std::vector<double>& ee_joint_pos,
                        const rviz_visual_tools::colors& color = rviz_visual_tools::CLEAR,
                        const std::string& ns = "end_effector")
  {
    return publishEEMarkers(convertPose(pose), ee_jmg, ee_joint_pos, color, ns);
  }
  bool publishEEMarkers(const Eigen::Isometry3d& 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")
  {
    return publishEEMarkers(pose, ee_jmg, {}, color, ns);
  }
  bool publishEEMarkers(const geometry_msgs::Pose& pose, const robot_model::JointModelGroup* ee_jmg,
                        const std::vector<double>& ee_joint_pos,
                        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 = "block",
                             double block_size = 0.1,
                             const rviz_visual_tools::colors& color = rviz_visual_tools::GREEN);

  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 publishCollisionCuboid(const Eigen::Isometry3d& pose, double width, double depth, double height,
                              const std::string& name, const rviz_visual_tools::colors& color);

  bool publishCollisionCuboid(const geometry_msgs::Pose& pose, double width, double depth, double height,
                              const std::string& name, const rviz_visual_tools::colors& color);

  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::Isometry3d& 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::Isometry3d& 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::Isometry3d& 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 z, double angle, double width, double height,
                           const std::string& name, moveit_msgs::CollisionObject& collision_obj);

  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 publishCollisionWall(double x, double y, double z, 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);

  RVIZ_VISUAL_TOOLS_DEPRECATED
  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)
  {
    return publishCollisionTable(x, y, 0, angle, width, height, depth, name, color);
  }

  bool publishCollisionTable(double x, double y, double z, 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::Isometry3d& offset);

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

  bool checkAndPublishCollision(const moveit::core::RobotState& robot_state,
                                const planning_scene::PlanningScene* planning_scene,
                                const rviz_visual_tools::colors& highlight_link_color = rviz_visual_tools::RED,
                                const rviz_visual_tools::colors& contact_point_color = rviz_visual_tools::PURPLE);

  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 publishContactPoints(const collision_detection::CollisionResult::ContactMap& contacts,
                            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);
  bool publishTrajectoryPath(const moveit_msgs::RobotTrajectory& trajectory_msg,
                             const moveit::core::RobotState& robot_state, bool blocking = false);
  bool publishTrajectoryPath(const moveit_msgs::RobotTrajectory& trajectory_msg,
                             const moveit_msgs::RobotState& robot_state, bool blocking = false);
  void publishTrajectoryPath(const moveit_msgs::DisplayTrajectory& display_trajectory_msg);

  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 = rviz_visual_tools::LIME_GREEN);
  bool publishTrajectoryLine(const robot_trajectory::RobotTrajectoryPtr& robot_trajectory,
                             const moveit::core::LinkModel* ee_parent_link,
                             const rviz_visual_tools::colors& color = rviz_visual_tools::LIME_GREEN);
  bool publishTrajectoryLine(const robot_trajectory::RobotTrajectory& robot_trajectory,
                             const moveit::core::LinkModel* ee_parent_link,
                             const rviz_visual_tools::colors& color = rviz_visual_tools::LIME_GREEN);

  bool publishTrajectoryLine(const moveit_msgs::RobotTrajectory& trajectory_msg,
                             const robot_model::JointModelGroup* arm_jmg,
                             const rviz_visual_tools::colors& color = rviz_visual_tools::LIME_GREEN);
  bool publishTrajectoryLine(const robot_trajectory::RobotTrajectoryPtr& robot_trajectory,
                             const robot_model::JointModelGroup* arm_jmg,
                             const rviz_visual_tools::colors& color = rviz_visual_tools::LIME_GREEN);
  bool publishTrajectoryLine(const robot_trajectory::RobotTrajectory& robot_trajectory,
                             const robot_model::JointModelGroup* arm_jmg,
                             const rviz_visual_tools::colors& color = rviz_visual_tools::LIME_GREEN);

  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::Isometry3d& 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 std::vector<double>& joint_positions, 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,
                         const std::vector<std::string>& highlight_links = {});
  bool publishRobotState(const moveit::core::RobotStatePtr& robot_state,
                         const rviz_visual_tools::colors& color = rviz_visual_tools::DEFAULT,
                         const std::vector<std::string>& highlight_links = {});
  void publishRobotState(const moveit_msgs::DisplayRobotState& display_robot_state_msg);

  bool hideRobot();

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

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

  planning_scene_monitor::PlanningSceneMonitorPtr getPlanningSceneMonitor();

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

protected:
  // Pointer to a Planning Scene Monitor
  planning_scene_monitor::PlanningSceneMonitorPtr psm_;

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

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

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

  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_Isometry3d> ee_poses_map_;
  std::map<const robot_model::JointModelGroup*, std::vector<double> > ee_joint_pos_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_ = false;
  Eigen::Isometry3d robot_state_root_offset_;

public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW  // http://eigen.tuxfamily.org/dox/group__TopicStructHavingEigenMembers.html
};                                 // class

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

}  // namespace moveit_visual_tools

#endif  // MOVEIT_VISUAL_TOOLS_MOVEIT_VISUAL_TOOLS_H