motion_planning_display.h

Go to the documentation of this file.

/*********************************************************************
 * Software License Agreement (BSD License)
 *
 *  Copyright (c) 2008, 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: Ioan Sucan, Dave Coleman, Adam Leeper, Sachin Chitta */
 
#pragma once
 
#include <rviz_common/display.hpp>
#include <rviz_common/panel_dock_widget.hpp>
#include <moveit/planning_scene_rviz_plugin/planning_scene_display.h>
#include <moveit/rviz_plugin_render_tools/trajectory_visualization.h>
 
#ifndef Q_MOC_RUN
#include <moveit/motion_planning_rviz_plugin/motion_planning_frame.h>
#include <moveit/robot_interaction/robot_interaction.h>
#include <moveit/robot_interaction/interaction_handler.h>
 
#include <moveit/planning_scene_monitor/planning_scene_monitor.h>
#include <moveit/kinematics_metrics/kinematics_metrics.h>
#include <moveit/dynamics_solver/dynamics_solver.h>
 
#include <rclcpp/rclcpp.hpp>
 
#include <std_msgs/msg/string.hpp>
#include <moveit_msgs/msg/display_trajectory.hpp>
#endif
 
#include <memory>
 
namespace Ogre
{
class SceneNode;
}
 
namespace rviz_rendering
{
class Shape;
class MovableText;
}  // namespace rviz_rendering
 
namespace rviz_common
{
namespace properties
{
class Property;
class StringProperty;
class BoolProperty;
class FloatProperty;
class RosTopicProperty;
class EditableEnumProperty;
class ColorProperty;
class MovableText;
}  // namespace properties
}  // namespace rviz_common
 
namespace rviz_default_plugins
{
namespace robot
{
class Robot;
}
}  // namespace rviz_default_plugins
 
namespace moveit_rviz_plugin
{
class MotionPlanningDisplay : public PlanningSceneDisplay
{
  Q_OBJECT
 
public:
  MotionPlanningDisplay();
 
  ~MotionPlanningDisplay() override;
 
  void load(const rviz_common::Config& config) override;
  void save(rviz_common::Config config) const override;
 
  void update(float wall_dt, float ros_dt) override;
  void reset() override;
 
  moveit::core::RobotStateConstPtr getQueryStartState() const
  {
    return query_start_state_->getState();
  }
 
  moveit::core::RobotStateConstPtr getQueryGoalState() const
  {
    return query_goal_state_->getState();
  }
 
  const moveit::core::RobotState& getPreviousState() const
  {
    return *previous_state_;
  }
 
  const robot_interaction::RobotInteractionPtr& getRobotInteraction() const
  {
    return robot_interaction_;
  }
 
  const robot_interaction::InteractionHandlerPtr& getQueryStartStateHandler() const
  {
    return query_start_state_;
  }
 
  const robot_interaction::InteractionHandlerPtr& getQueryGoalStateHandler() const
  {
    return query_goal_state_;
  }
 
  void dropVisualizedTrajectory()
  {
    trajectory_visual_->dropTrajectory();
  }
 
  void setQueryStartState(const moveit::core::RobotState& start);
  void setQueryGoalState(const moveit::core::RobotState& goal);
 
  void updateQueryStates(const moveit::core::RobotState& current_state);
  void updateQueryStartState();
  void updateQueryGoalState();
  void rememberPreviousStartState();
 
  void useApproximateIK(bool flag);
 
  // Pick Place
  void clearPlaceLocationsDisplay();
  void visualizePlaceLocations(const std::vector<geometry_msgs::msg::PoseStamped>& place_poses);
  std::vector<std::shared_ptr<rviz_rendering::Shape> > place_locations_display_;
 
  std::string getCurrentPlanningGroup() const;
 
  void changePlanningGroup(const std::string& group);
 
  void addStatusText(const std::string& text);
  void addStatusText(const std::vector<std::string>& text);
  void setStatusTextColor(const QColor& color);
  void resetStatusTextColor();
 
  void toggleSelectPlanningGroupSubscription(bool enable);
 
Q_SIGNALS:
  // signals issued when start/goal states of a query changed
  void queryStartStateChanged();
  void queryGoalStateChanged();
 
private Q_SLOTS:
 
  // ******************************************************************************************
  // Slot Event Functions
  // ******************************************************************************************
 
  void changedQueryStartState();
  void changedQueryGoalState();
  void changedQueryMarkerScale();
  void changedQueryStartColor();
  void changedQueryGoalColor();
  void changedQueryStartAlpha();
  void changedQueryGoalAlpha();
  void changedQueryCollidingLinkColor();
  void changedQueryJointViolationColor();
  void changedAttachedBodyColor() override;
  void changedPlanningGroup();
  void changedShowWeightLimit();
  void changedShowManipulabilityIndex();
  void changedShowManipulability();
  void changedShowJointTorques();
  void changedMetricsSetPayload();
  void changedMetricsTextHeight();
  void changedWorkspace();
  void resetInteractiveMarkers();
  void motionPanelVisibilityChange(bool enable);
 
protected:
  enum LinkDisplayStatus
  {
    COLLISION_LINK,
    OUTSIDE_BOUNDS_LINK
  };
 
  void clearRobotModel() override;
  void onRobotModelLoaded() override;
  void onNewPlanningSceneState() override;
  void onSceneMonitorReceivedUpdate(planning_scene_monitor::PlanningSceneMonitor::SceneUpdateType update_type) override;
  void updateInternal(float wall_dt, float ros_dt) override;
 
  void renderWorkspaceBox();
  void updateLinkColors();
 
  void displayTable(const std::map<std::string, double>& values, const Ogre::ColourValue& color,
                    const Ogre::Vector3& pos, const Ogre::Quaternion& orient);
  void displayMetrics(bool start);
 
  void executeMainLoopJobs();
  void publishInteractiveMarkers(bool pose_update);
 
  void recomputeQueryStartStateMetrics();
  void recomputeQueryGoalStateMetrics();
  void drawQueryStartState();
  void drawQueryGoalState();
  void scheduleDrawQueryStartState(robot_interaction::InteractionHandler* handler, bool error_state_changed);
  void scheduleDrawQueryGoalState(robot_interaction::InteractionHandler* handler, bool error_state_changed);
 
  bool isIKSolutionCollisionFree(moveit::core::RobotState* state, const moveit::core::JointModelGroup* group,
                                 const double* ik_solution) const;
 
  void computeMetrics(bool start, const std::string& group, double payload);
  void computeMetricsInternal(std::map<std::string, double>& metrics,
                              const robot_interaction::EndEffectorInteraction& eef,
                              const moveit::core::RobotState& state, double payload);
  void updateStateExceptModified(moveit::core::RobotState& dest, const moveit::core::RobotState& src);
  void updateBackgroundJobProgressBar();
  void backgroundJobUpdate(moveit::tools::BackgroundProcessing::JobEvent event, const std::string& jobname);
 
  void setQueryStateHelper(bool use_start_state, const std::string& v);
  void populateMenuHandler(std::shared_ptr<interactive_markers::MenuHandler>& mh);
 
  void selectPlanningGroupCallback(const std_msgs::msg::String::ConstSharedPtr& msg);
 
  // overrides from Display
  void onInitialize() override;
  void onEnable() override;
  void onDisable() override;
  void fixedFrameChanged() override;
 
  RobotStateVisualizationPtr query_robot_start_;  
  RobotStateVisualizationPtr query_robot_goal_;   
 
  Ogre::SceneNode* text_display_scene_node_;  
  bool text_display_for_start_;               
  rviz_rendering::MovableText* text_to_display_;
 
  rclcpp::Subscription<std_msgs::msg::String>::SharedPtr planning_group_sub_;
 
  // render the workspace box
  std::unique_ptr<rviz_rendering::Shape> workspace_box_;
 
  // the planning frame
  MotionPlanningFrame* frame_;
  rviz_common::PanelDockWidget* frame_dock_;
 
  // robot interaction
  robot_interaction::RobotInteractionPtr robot_interaction_;
  robot_interaction::InteractionHandlerPtr query_start_state_;
  robot_interaction::InteractionHandlerPtr query_goal_state_;
  std::shared_ptr<interactive_markers::MenuHandler> menu_handler_start_;
  std::shared_ptr<interactive_markers::MenuHandler> menu_handler_goal_;
  std::map<std::string, LinkDisplayStatus> status_links_start_;
  std::map<std::string, LinkDisplayStatus> status_links_goal_;
  moveit::core::RobotStatePtr previous_state_;
 
  std::set<std::string> modified_groups_;
 
  std::map<std::pair<bool, std::string>, std::map<std::string, double> > computed_metrics_;
  std::map<std::string, bool> position_only_ik_;
 
  // Metric calculations
  kinematics_metrics::KinematicsMetricsPtr kinematics_metrics_;
  std::map<std::string, dynamics_solver::DynamicsSolverPtr> dynamics_solver_;
  std::mutex update_metrics_lock_;
 
  // The trajectory playback component
  TrajectoryVisualizationPtr trajectory_visual_;
 
  // properties to show on side panel
  rviz_common::properties::Property* path_category_;
  rviz_common::properties::Property* plan_category_;
  rviz_common::properties::Property* metrics_category_;
 
  rviz_common::properties::EditableEnumProperty* planning_group_property_;
  rviz_common::properties::BoolProperty* query_start_state_property_;
  rviz_common::properties::BoolProperty* query_goal_state_property_;
  rviz_common::properties::FloatProperty* query_marker_scale_property_;
  rviz_common::properties::ColorProperty* query_start_color_property_;
  rviz_common::properties::ColorProperty* query_goal_color_property_;
  rviz_common::properties::FloatProperty* query_start_alpha_property_;
  rviz_common::properties::FloatProperty* query_goal_alpha_property_;
  rviz_common::properties::ColorProperty* query_colliding_link_color_property_;
  rviz_common::properties::ColorProperty* query_outside_joint_limits_link_color_property_;
 
  rviz_common::properties::BoolProperty* compute_weight_limit_property_;
  rviz_common::properties::BoolProperty* show_manipulability_index_property_;
  rviz_common::properties::BoolProperty* show_manipulability_property_;
  rviz_common::properties::BoolProperty* show_joint_torques_property_;
  rviz_common::properties::FloatProperty* metrics_set_payload_property_;
  rviz_common::properties::FloatProperty* metrics_text_height_property_;
  rviz_common::properties::BoolProperty* show_workspace_property_;
 
  rviz_common::Display* int_marker_display_;
};
 
}  // namespace moveit_rviz_plugin