Program Listing for File moveit_visual_tools.h

↰ Return to documentation for file (/tmp/ws/src/moveit_visual_tools/include/moveit_visual_tools/moveit_visual_tools.h)

// Copyright 2015 University of Colorado, Boulder
//
// 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 copyright holder 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 HOLDER 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.hpp>

// MoveIt
#include <moveit/planning_scene_monitor/planning_scene_monitor.h>

// MoveIt Messages
#include <moveit_msgs/msg/grasp.hpp>
#include <moveit_msgs/msg/display_robot_state.hpp>
#include <moveit_msgs/msg/workspace_parameters.hpp>
#include <moveit_msgs/msg/display_robot_state.hpp>
#include <moveit_msgs/msg/display_trajectory.hpp>

// ROS Messages
#include <trajectory_msgs/msg/joint_trajectory.hpp>
#include <graph_msgs/msg/geometry_graph.hpp>

// C++
#include <map>
#include <string>
#include <utility>
#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 rclcpp::Node::SharedPtr& node);

  MoveItVisualTools(const rclcpp::Node::SharedPtr& node, const std::string& base_frame, const std::string& marker_topic,
                    planning_scene_monitor::PlanningSceneMonitorPtr psm);

  MoveItVisualTools(const rclcpp::Node::SharedPtr& node, const std::string& base_frame,
                    const std::string& marker_topic = rviz_visual_tools::RVIZ_MARKER_TOPIC,
                    moveit::core::RobotModelConstPtr robot_model = moveit::core::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::msg::CollisionObject& msg,
                                 const rviz_visual_tools::Colors& color = rviz_visual_tools::GREEN);

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

  bool moveCollisionObject(const Eigen::Isometry3d& pose, const std::string& name,
                           const rviz_visual_tools::Colors& color);
  bool moveCollisionObject(const geometry_msgs::msg::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 moveit::core::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_ = std::move(psm);
  }

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

  bool publishEEMarkers(const Eigen::Isometry3d& pose, const moveit::core::JointModelGroup* ee_jmg,
                        const std::vector<double>& ee_joint_pos,
                        const rviz_visual_tools::Colors& color = rviz_visual_tools::DEFAULT,
                        const std::string& ns = "end_effector")
  {
    return publishEEMarkers(convertPose(pose), ee_jmg, ee_joint_pos, color, ns);
  }
  bool publishEEMarkers(const Eigen::Isometry3d& pose, const moveit::core::JointModelGroup* ee_jmg,
                        const rviz_visual_tools::Colors& color = rviz_visual_tools::DEFAULT,
                        const std::string& ns = "end_effector")
  {
    return publishEEMarkers(convertPose(pose), ee_jmg, {}, color, ns);
  }
  bool publishEEMarkers(const geometry_msgs::msg::Pose& pose, const moveit::core::JointModelGroup* ee_jmg,
                        const rviz_visual_tools::Colors& color = rviz_visual_tools::DEFAULT,
                        const std::string& ns = "end_effector")
  {
    return publishEEMarkers(pose, ee_jmg, {}, color, ns);
  }
  bool publishEEMarkers(const geometry_msgs::msg::Pose& pose, const moveit::core::JointModelGroup* ee_jmg,
                        const std::vector<double>& ee_joint_pos,
                        const rviz_visual_tools::Colors& color = rviz_visual_tools::DEFAULT,
                        const std::string& ns = "end_effector");

  bool publishGrasps(const std::vector<moveit_msgs::msg::Grasp>& possible_grasps,
                     const moveit::core::JointModelGroup* ee_jmg, double animate_speed = 0.1 /* seconds */);

  bool publishAnimatedGrasps(const std::vector<moveit_msgs::msg::Grasp>& possible_grasps,
                             const moveit::core::JointModelGroup* ee_jmg, double animate_speed = 0.01 /* seconds */);

  bool publishAnimatedGrasp(const moveit_msgs::msg::Grasp& grasp, const moveit::core::JointModelGroup* ee_jmg,
                            double animate_speed /* seconds */);

  bool publishIKSolutions(const std::vector<trajectory_msgs::msg::JointTrajectoryPoint>& ik_solutions,
                          const moveit::core::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::msg::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::msg::Point& point1, const geometry_msgs::msg::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::msg::Pose& pose, double width, double depth, double height,
                              const std::string& name, const rviz_visual_tools::Colors& color);

  bool publishCollisionCuboid(const Eigen::Isometry3d& pose, const Eigen::Vector3d& size, const std::string& name,
                              const rviz_visual_tools::Colors& color)
  {
    return publishCollisionCuboid(pose, size.x(), size.y(), size.z(), name, color);
  }
  bool publishCollisionCuboid(const geometry_msgs::msg::Pose& pose, const geometry_msgs::msg::Vector3& size,
                              const std::string& name, const rviz_visual_tools::Colors& color)
  {
    return publishCollisionCuboid(pose, size.x, size.y, size.z, name, color);
  }

  bool publishCollisionCylinder(const geometry_msgs::msg::Point& a, const geometry_msgs::msg::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::msg::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::msg::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::msg::Mesh& mesh_msg,
                            const rviz_visual_tools::Colors& color = rviz_visual_tools::GREEN);
  bool publishCollisionMesh(const geometry_msgs::msg::Pose& object_pose, const std::string& object_name,
                            const shape_msgs::msg::Mesh& mesh_msg,
                            const rviz_visual_tools::Colors& color = rviz_visual_tools::GREEN);

  bool publishCollisionGraph(const graph_msgs::msg::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::msg::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);

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

  bool publishTrajectoryLine(const moveit_msgs::msg::RobotTrajectory& trajectory_msg,
                             const moveit::core::LinkModel* ee_parent_link,
                             const moveit::core::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::msg::RobotTrajectory& trajectory_msg,
                             const moveit::core::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::JointModelGroup* arm_jmg,
                             const rviz_visual_tools::Colors& color = rviz_visual_tools::LIME_GREEN);
  bool publishTrajectoryLine(const robot_trajectory::RobotTrajectory& robot_trajectory,
                             const moveit::core::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::msg::JointTrajectoryPoint& trajectory_pt,
                         const moveit::core::JointModelGroup* jmg,
                         const rviz_visual_tools::Colors& color = rviz_visual_tools::DEFAULT);

  bool publishRobotState(const std::vector<double>& joint_positions, const moveit::core::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::msg::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 manual_trigger_update_ = false;

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

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

  // ROS publishers
  rclcpp::Publisher<moveit_msgs::msg::DisplayTrajectory>::SharedPtr pub_display_path_;  // for MoveIt trajectories
  rclcpp::Publisher<moveit_msgs::msg::DisplayRobotState>::SharedPtr pub_robot_state_;   // publish a RobotState message

  // ROS Node
  rclcpp::Node::SharedPtr node_;

  robot_model_loader::RobotModelLoaderPtr rm_loader_;  // so that we can specify our own options

  // End Effector Markers
  std::map<const moveit::core::JointModelGroup*, visualization_msgs::msg::MarkerArray> ee_markers_map_;
  std::map<const moveit::core::JointModelGroup*, EigenSTL::vector_Isometry3d> ee_poses_map_;
  std::map<const moveit::core::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::msg::DisplayRobotState> display_robot_msgs_;

  // 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