visual_tools.h

Go to the documentation of this file.

/*********************************************************************
 * Software License Agreement (BSD License)
 *
 *  Copyright (c) 2013, 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
 *
 *          See README.md for developers notes.
 *
 */

#ifndef MOVEIT_VISUAL_TOOLS__VISUAL_TOOLS_H_
#define MOVEIT_VISUAL_TOOLS__VISUAL_TOOLS_H_

// Rviz
#include <visualization_msgs/Marker.h>
#include <visualization_msgs/MarkerArray.h>

// MoveIt
#include <moveit/planning_scene_monitor/planning_scene_monitor.h>
#include <moveit_msgs/Grasp.h>
#include <moveit_msgs/DisplayRobotState.h>
#include <moveit/macros/deprecation.h>

// Boost
#include <boost/shared_ptr.hpp>

// Messages
#include <std_msgs/ColorRGBA.h>
#include <graph_msgs/GeometryGraph.h>
#include <geometry_msgs/PoseArray.h>
#include <geometry_msgs/Polygon.h>
#include <trajectory_msgs/JointTrajectory.h>

namespace moveit_visual_tools
{

// Default constants
static const std::string ROBOT_DESCRIPTION = "robot_description";
static const std::string COLLISION_TOPIC = "/collision_object";
static const std::string ATTACHED_COLLISION_TOPIC = "/attached_collision_object";
static const std::string RVIZ_MARKER_TOPIC = "/end_effector_marker";
static const std::string PLANNING_SCENE_TOPIC = "/move_group/monitored_planning_scene";
static const std::string DISPLAY_PLANNED_PATH_TOPIC = "/move_group/display_planned_path";
static const std::string DISPLAY_ROBOT_STATE_TOPIC = "/move_group/robot_state";

// Note: when adding new colors to rviz_colors, also add them to getRandColor() function
enum rviz_colors { RED, 
                   GREEN, 
                   BLUE, 
                   GREY, 
                   WHITE, 
                   ORANGE, 
                   BLACK, 
                   YELLOW, 
                   PURPLE, 
                   TRANSLUCENT, 
                   TRANSLUCENT2,
                   RAND };

enum rviz_scales { XXSMALL,
                   XSMALL,
                   SMALL,
                   REGULAR,
                   LARGE, xLARGE, xxLARGE, xxxLARGE,
                   XLARGE,
                   XXLARGE };

class VisualTools
{
protected:

  // A shared node handle
  ros::NodeHandle nh_;

  // ROS publishers
  ros::Publisher pub_rviz_marker_; // for rviz visualization markers
  ros::Publisher pub_collision_obj_; // for MoveIt collision objects
  ros::Publisher pub_attach_collision_obj_; // for MoveIt attached objects
  ros::Publisher pub_display_path_; // for MoveIt trajectories
  ros::Publisher pub_planning_scene_diff_; // for adding and removing collision objects
  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

  // Strings
  std::string marker_topic_; // topic to publish to rviz
  std::string base_frame_; // name of base link of robot

  // TODO rename this
  double floor_to_base_height_; // allows an offset between base link and floor where objects are built

  // Duration to have Rviz markers persist, 0 for infinity
  ros::Duration marker_lifetime_;

  // End Effector Markers
  visualization_msgs::MarkerArray ee_marker_array_;
  tf::Pose tf_root_to_link_;
  geometry_msgs::Pose grasp_pose_to_eef_pose_; // Convert generic grasp pose to this end effector's frame of reference
  std::vector<geometry_msgs::Pose> marker_poses_;

  // Library settings
  bool muted_; // Whether to actually publish to rviz or not
  double alpha_; // opacity of all markers
  double global_scale_; // allow all markers to be increased by a constanct factor

  // Cached Rviz markers
  visualization_msgs::Marker arrow_marker_;
  visualization_msgs::Marker sphere_marker_;
  visualization_msgs::Marker block_marker_;
  visualization_msgs::Marker cylinder_marker_;
  visualization_msgs::Marker text_marker_;
  visualization_msgs::Marker rectangle_marker_;
  visualization_msgs::Marker line_marker_;
  visualization_msgs::Marker path_marker_;
  visualization_msgs::Marker spheres_marker_;
  visualization_msgs::Marker reset_marker_;

  // MoveIt cached markers
  moveit_msgs::DisplayRobotState display_robot_msg_;

  // MoveIt cached objects
  robot_state::RobotStatePtr shared_robot_state_; // Note: call loadSharedRobotState() before using this
  robot_state::RobotModelConstPtr robot_model_;

private:
  void initialize();

public:

  VisualTools(const std::string& base_frame,
              const std::string& marker_topic,
              planning_scene_monitor::PlanningSceneMonitorPtr planning_scene_monitor);

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

  ~VisualTools() {};

  void deleteAllMarkers();

  void resetMarkerCounts();

  bool loadRvizMarkers();

  bool loadPlanningSceneMonitor();

  bool processCollisionObjectMsg(moveit_msgs::CollisionObject msg);

  bool loadSharedRobotState();

  bool loadRobotMarkers();

  bool loadEEMarker(const std::string& ee_group_name, const std::string& planning_group);

  void loadMarkerPub();
  void loadCollisionPub();
  void loadAttachedPub();
  void loadPlanningPub();
  void loadTrajectoryPub();
  void loadRobotStatePub(const std::string &marker_topic = DISPLAY_ROBOT_STATE_TOPIC);

  bool isMuted()
  {
    return muted_;
  }

  void setMuted(bool muted)
  {
    muted_ = muted;
  }

  void setFloorToBaseHeight(double floor_to_base_height);

  void setGraspPoseToEEFPose(geometry_msgs::Pose grasp_pose_to_eef_pose);

  void setAlpha(double alpha)
  {
    alpha_ = alpha;
  }

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

  void setLifetime(double lifetime);

  const rviz_colors getRandColor();

  std_msgs::ColorRGBA getColor(const rviz_colors &color);

  geometry_msgs::Vector3 getScale(const rviz_scales &scale, bool arrow_scale = false, double marker_scale = 1.0);

  Eigen::Affine3d getVectorBetweenPoints(Eigen::Vector3d a, Eigen::Vector3d b);

  Eigen::Vector3d getCenterPoint(Eigen::Vector3d a, Eigen::Vector3d b);

  const std::string getBaseFrame()
  {
    return base_frame_;
  }
  MOVEIT_DEPRECATED const std::string getBaseLink()
  {
    return base_frame_;
  }

  void setBaseFrame(const std::string& base_frame)
  {
    base_frame_ = base_frame;
    loadRvizMarkers();
  }

  double getGlobalScale()
  {
    return global_scale_;
  }

  void setGlobalScale(double global_scale)
  {
    global_scale_ = global_scale;
  }

  bool publishEEMarkers(const geometry_msgs::Pose &pose, const rviz_colors &color = WHITE, const std::string &ns="end_effector");

  bool publishSphere(const Eigen::Affine3d &pose, const rviz_colors color = BLUE, const rviz_scales scale = REGULAR, const std::string& ns = "Sphere");
  bool publishSphere(const Eigen::Vector3d &point, const rviz_colors color = BLUE, const rviz_scales scale = REGULAR, const std::string& ns = "Sphere");
  bool publishSphere(const geometry_msgs::Point &point, const rviz_colors color = BLUE, const rviz_scales scale = REGULAR, const std::string& ns = "Sphere");
  bool publishSphere(const geometry_msgs::Pose &pose, const rviz_colors color = BLUE, const rviz_scales scale = REGULAR, const std::string& ns = "Sphere");
  bool publishSphere(const geometry_msgs::Pose &pose, const rviz_colors color, const double scale, const std::string& ns = "Sphere");
  bool publishSphere(const geometry_msgs::Pose &pose, const rviz_colors color, const geometry_msgs::Vector3 scale, const std::string& ns = "Sphere");

  bool publishArrow(const Eigen::Affine3d &pose, const rviz_colors color = BLUE, const rviz_scales scale = REGULAR);
  bool publishArrow(const geometry_msgs::Pose &pose, const rviz_colors color = BLUE, const rviz_scales scale = REGULAR);

  bool publishRectangle(const geometry_msgs::Point &point1, const geometry_msgs::Point &point2, const rviz_colors color = BLUE);

  bool publishLine(const geometry_msgs::Point &point1, const geometry_msgs::Point &point2,
                   const rviz_colors color = BLUE, const rviz_scales scale = REGULAR);

  bool publishPath(const std::vector<geometry_msgs::Point> &path, const rviz_colors color = RED, const rviz_scales scale = REGULAR,
                   const std::string& ns = "Path");

  bool publishPolygon(const geometry_msgs::Polygon &polygon, const rviz_colors color = RED, const rviz_scales scale = REGULAR,
                      const std::string& ns = "Polygon");

  bool publishBlock(const geometry_msgs::Pose &pose, const rviz_colors color = BLUE, const double &block_size = 0.1);

  bool publishCylinder(const geometry_msgs::Pose &pose, const rviz_colors color = BLUE, double height = 0.1, double radius = 0.1);

  bool publishGraph(const graph_msgs::GeometryGraph &graph, const rviz_colors color, double radius);


  bool publishSpheres(const std::vector<geometry_msgs::Point> &points, const rviz_colors color = BLUE, const rviz_scales scale = REGULAR,
                      const std::string& ns = "Spheres");

  bool publishText(const geometry_msgs::Pose &pose, const std::string &text,
                   const rviz_colors &color = WHITE, const rviz_scales scale = REGULAR);

  bool publishText(const geometry_msgs::Pose &pose, const std::string &text,
                   const rviz_colors &color, const geometry_msgs::Vector3 scale, bool static_id = true);

  bool publishMarker(const visualization_msgs::Marker &marker);

  bool publishGrasps(const std::vector<moveit_msgs::Grasp>& possible_grasps,
                     const std::string &ee_parent_link);

  bool publishAnimatedGrasps(const std::vector<moveit_msgs::Grasp>& possible_grasps,
                             const std::string &ee_parent_link, double animate_speed = 0.01);

  bool publishAnimatedGrasp(const moveit_msgs::Grasp &grasp, const std::string &ee_parent_link, double animate_speed);

  bool publishIKSolutions(const std::vector<trajectory_msgs::JointTrajectoryPoint> &ik_solutions,
                          const std::string& planning_group, double display_time = 0.4);

  MOVEIT_DEPRECATED bool removeAllCollisionObjects()
  {
    publishRemoveAllCollisionObjects();
  }
  bool publishRemoveAllCollisionObjects();

  bool removeAllCollisionObjectsPS();

  bool cleanupCO(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, std::string plane_name);

  bool publishCollisionBlock(geometry_msgs::Pose block_pose, std::string block_name, double block_size);

  bool publishCollisionCylinder(geometry_msgs::Point a, geometry_msgs::Point b, std::string object_name, double radius);
  bool publishCollisionCylinder(Eigen::Vector3d a, Eigen::Vector3d b, std::string object_name, double radius);

  bool publishCollisionCylinder(Eigen::Affine3d object_pose, std::string object_name, double radius, double height);
  bool publishCollisionCylinder(geometry_msgs::Pose object_pose, std::string object_name, double radius, double height);

  bool publishCollisionGraph(const graph_msgs::GeometryGraph &graph, const std::string &object_name, double radius);

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

  bool publishCollisionWall(double x, double y, double angle, double width, const std::string name);

  bool publishCollisionTable(double x, double y, double angle, double width, double height,
                             double depth, const std::string name);

  bool loadCollisionSceneFromFile(const std::string &path);

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

  bool publishTrajectoryPath(const robot_trajectory::RobotTrajectory& trajectory, bool blocking = false);
  bool publishTrajectoryPath(const moveit_msgs::RobotTrajectory& trajectory_msg, bool blocking = false);

  bool publishRobotState(const robot_state::RobotState &robot_state);
  bool publishRobotState(const robot_state::RobotStatePtr &robot_state);

  bool publishRobotState(const trajectory_msgs::JointTrajectoryPoint& trajectory_pt, const std::string &group_name);

  bool hideRobot();

  bool publishTest();

  static geometry_msgs::Pose convertPose(const Eigen::Affine3d &pose);

  static Eigen::Affine3d convertPose(const geometry_msgs::Pose &pose);

  static Eigen::Affine3d convertPoint32ToPose(const geometry_msgs::Point32 &point);

  static geometry_msgs::Pose convertPointToPose(const geometry_msgs::Point &point);

  static geometry_msgs::Point convertPoseToPoint(const Eigen::Affine3d &pose);

  static Eigen::Vector3d convertPoint(const geometry_msgs::Point &point);

  static Eigen::Vector3d convertPoint32(const geometry_msgs::Point32 &point);

  static geometry_msgs::Point32 convertPoint32(const Eigen::Vector3d &point);

  void generateRandomPose(geometry_msgs::Pose& pose);

  static double dRand(double dMin, double dMax);
  static float fRand(float dMin, float dMax);
  static int iRand(int dMin, int dMax);

  void print();

private:

  planning_scene_monitor::PlanningSceneMonitorPtr getPlanningSceneMonitor();


}; // class

typedef boost::shared_ptr<VisualTools> VisualToolsPtr;
typedef boost::shared_ptr<const VisualTools> VisualToolsConstPtr;

} // namespace

#endif