motion_planning_display.h
Go to the documentation of this file.
00001 /*********************************************************************
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Copyright (c) 2008, Willow Garage, Inc.
00005  *  All rights reserved.
00006  *
00007  *  Redistribution and use in source and binary forms, with or without
00008  *  modification, are permitted provided that the following conditions
00009  *  are met:
00010  *
00011  *   * Redistributions of source code must retain the above copyright
00012  *     notice, this list of conditions and the following disclaimer.
00013  *   * Redistributions in binary form must reproduce the above
00014  *     copyright notice, this list of conditions and the following
00015  *     disclaimer in the documentation and/or other materials provided
00016  *     with the distribution.
00017  *   * Neither the name of Willow Garage nor the names of its
00018  *     contributors may be used to endorse or promote products derived
00019  *     from this software without specific prior written permission.
00020  *
00021  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032  *  POSSIBILITY OF SUCH DAMAGE.
00033  *********************************************************************/
00034 
00035 /* Author: Ioan Sucan, Dave Coleman, Adam Leeper, Sachin Chitta */
00036 
00037 #ifndef MOVEIT_MOTION_PLANNING_RVIZ_PLUGIN_MOTION_PLANNING_DISPLAY_
00038 #define MOVEIT_MOTION_PLANNING_RVIZ_PLUGIN_MOTION_PLANNING_DISPLAY_
00039 
00040 #include <rviz/display.h>
00041 #include <rviz/selection/selection_manager.h>
00042 #include <rviz/panel_dock_widget.h>
00043 #include <moveit/planning_scene_rviz_plugin/planning_scene_display.h>
00044 #include <moveit/rviz_plugin_render_tools/trajectory_visualization.h>
00045 #include <std_msgs/String.h>
00046 
00047 #ifndef Q_MOC_RUN
00048 #include <moveit/motion_planning_rviz_plugin/motion_planning_frame.h>
00049 #include <moveit/robot_interaction/robot_interaction.h>
00050 #include <moveit/robot_interaction/interaction_handler.h>
00051 
00052 #include <moveit/planning_scene_monitor/planning_scene_monitor.h>
00053 #include <moveit/kinematics_metrics/kinematics_metrics.h>
00054 #include <moveit/dynamics_solver/dynamics_solver.h>
00055 #include <ros/ros.h>
00056 #endif
00057 
00058 #include <moveit_msgs/DisplayTrajectory.h>
00059 
00060 namespace Ogre
00061 {
00062 class SceneNode;
00063 }
00064 
00065 namespace rviz
00066 {
00067 class Robot;
00068 class Shape;
00069 class Property;
00070 class StringProperty;
00071 class BoolProperty;
00072 class FloatProperty;
00073 class RosTopicProperty;
00074 class EditableEnumProperty;
00075 class ColorProperty;
00076 class MovableText;
00077 }
00078 
00079 namespace moveit_rviz_plugin
00080 {
00081 class MotionPlanningDisplay : public PlanningSceneDisplay
00082 {
00083   Q_OBJECT
00084 
00085 public:
00086   MotionPlanningDisplay();
00087 
00088   virtual ~MotionPlanningDisplay();
00089 
00090   virtual void load(const rviz::Config& config);
00091   virtual void save(rviz::Config config) const;
00092 
00093   virtual void update(float wall_dt, float ros_dt);
00094   virtual void reset();
00095 
00096   void setName(const QString& name);
00097 
00098   robot_state::RobotStateConstPtr getQueryStartState() const
00099   {
00100     return query_start_state_->getState();
00101   }
00102 
00103   robot_state::RobotStateConstPtr getQueryGoalState() const
00104   {
00105     return query_goal_state_->getState();
00106   }
00107 
00108   const robot_interaction::RobotInteractionPtr& getRobotInteraction() const
00109   {
00110     return robot_interaction_;
00111   }
00112 
00113   const robot_interaction::RobotInteraction::InteractionHandlerPtr& getQueryStartStateHandler() const
00114   {
00115     return query_start_state_;
00116   }
00117 
00118   const robot_interaction::RobotInteraction::InteractionHandlerPtr& getQueryGoalStateHandler() const
00119   {
00120     return query_goal_state_;
00121   }
00122 
00123   void dropVisualizedTrajectory()
00124   {
00125     trajectory_visual_->dropTrajectory();
00126   }
00127 
00128   void setQueryStartState(const robot_state::RobotState& start);
00129   void setQueryGoalState(const robot_state::RobotState& goal);
00130 
00131   void updateQueryStartState();
00132   void updateQueryGoalState();
00133 
00134   void useApproximateIK(bool flag);
00135 
00136   // Pick Place
00137   void clearPlaceLocationsDisplay();
00138   void visualizePlaceLocations(const std::vector<geometry_msgs::PoseStamped>& place_poses);
00139   std::vector<boost::shared_ptr<rviz::Shape> > place_locations_display_;
00140 
00141   std::string getCurrentPlanningGroup() const;
00142 
00143   void changePlanningGroup(const std::string& group);
00144 
00145   void addStatusText(const std::string& text);
00146   void addStatusText(const std::vector<std::string>& text);
00147   void setStatusTextColor(const QColor& color);
00148   void resetStatusTextColor();
00149 
00150   void toggleSelectPlanningGroupSubscription(bool enable);
00151 
00152 private Q_SLOTS:
00153 
00154   // ******************************************************************************************
00155   // Slot Event Functions
00156   // ******************************************************************************************
00157 
00158   void changedQueryStartState();
00159   void changedQueryGoalState();
00160   void changedQueryMarkerScale();
00161   void changedQueryStartColor();
00162   void changedQueryGoalColor();
00163   void changedQueryStartAlpha();
00164   void changedQueryGoalAlpha();
00165   void changedQueryCollidingLinkColor();
00166   void changedQueryJointViolationColor();
00167   void changedPlanningGroup();
00168   void changedShowWeightLimit();
00169   void changedShowManipulabilityIndex();
00170   void changedShowManipulability();
00171   void changedShowJointTorques();
00172   void changedMetricsSetPayload();
00173   void changedMetricsTextHeight();
00174   void changedWorkspace();
00175   void resetInteractiveMarkers();
00176   void motionPanelVisibilityChange(bool enable);
00177 
00178 protected:
00179   enum LinkDisplayStatus
00180   {
00181     COLLISION_LINK,
00182     OUTSIDE_BOUNDS_LINK
00183   };
00184 
00185   virtual void onRobotModelLoaded();
00186   virtual void onSceneMonitorReceivedUpdate(planning_scene_monitor::PlanningSceneMonitor::SceneUpdateType update_type);
00187   virtual void updateInternal(float wall_dt, float ros_dt);
00188 
00189   void renderWorkspaceBox();
00190   void updateLinkColors();
00191 
00192   void displayTable(const std::map<std::string, double>& values, const Ogre::ColourValue& color,
00193                     const Ogre::Vector3& pos, const Ogre::Quaternion& orient);
00194   void displayMetrics(bool start);
00195 
00196   void executeMainLoopJobs();
00197   void publishInteractiveMarkers(bool pose_update);
00198 
00199   void recomputeQueryStartStateMetrics();
00200   void recomputeQueryGoalStateMetrics();
00201   void drawQueryStartState();
00202   void drawQueryGoalState();
00203   void scheduleDrawQueryStartState(robot_interaction::RobotInteraction::InteractionHandler* handler,
00204                                    bool error_state_changed);
00205   void scheduleDrawQueryGoalState(robot_interaction::RobotInteraction::InteractionHandler* handler,
00206                                   bool error_state_changed);
00207 
00208   bool isIKSolutionCollisionFree(robot_state::RobotState* state, const robot_state::JointModelGroup* group,
00209                                  const double* ik_solution) const;
00210 
00211   void computeMetrics(bool start, const std::string& group, double payload);
00212   void computeMetricsInternal(std::map<std::string, double>& metrics,
00213                               const robot_interaction::RobotInteraction::EndEffector& eef,
00214                               const robot_state::RobotState& state, double payload);
00215   void updateStateExceptModified(robot_state::RobotState& dest, const robot_state::RobotState& src);
00216   void updateBackgroundJobProgressBar();
00217   void backgroundJobUpdate(moveit::tools::BackgroundProcessing::JobEvent event, const std::string& jobname);
00218 
00219   void setQueryStateHelper(bool use_start_state, const std::string& v);
00220   void populateMenuHandler(boost::shared_ptr<interactive_markers::MenuHandler>& mh);
00221 
00222   void selectPlanningGroupCallback(const std_msgs::StringConstPtr& msg);
00223 
00224   // overrides from Display
00225   virtual void onInitialize();
00226   virtual void onEnable();
00227   virtual void onDisable();
00228   virtual void fixedFrameChanged();
00229 
00230   RobotStateVisualizationPtr query_robot_start_;  
00231   RobotStateVisualizationPtr query_robot_goal_;   
00232 
00233   Ogre::SceneNode* text_display_scene_node_;  
00234   bool text_display_for_start_;               
00235   rviz::MovableText* text_to_display_;
00236 
00237   ros::Subscriber planning_group_sub_;
00238   ros::NodeHandle private_handle_, node_handle_;
00239 
00240   // render the workspace box
00241   boost::scoped_ptr<rviz::Shape> workspace_box_;
00242 
00243   // the planning frame
00244   MotionPlanningFrame* frame_;
00245   rviz::PanelDockWidget* frame_dock_;
00246 
00247   // robot interaction
00248   robot_interaction::RobotInteractionPtr robot_interaction_;
00249   robot_interaction::RobotInteraction::InteractionHandlerPtr query_start_state_;
00250   robot_interaction::RobotInteraction::InteractionHandlerPtr query_goal_state_;
00251   boost::shared_ptr<interactive_markers::MenuHandler> menu_handler_start_;
00252   boost::shared_ptr<interactive_markers::MenuHandler> menu_handler_goal_;
00253   std::map<std::string, LinkDisplayStatus> status_links_start_;
00254   std::map<std::string, LinkDisplayStatus> status_links_goal_;
00255 
00258   std::set<std::string> modified_groups_;
00259 
00262   std::map<std::pair<bool, std::string>, std::map<std::string, double> > computed_metrics_;
00264   std::map<std::string, bool> position_only_ik_;
00265 
00266   // Metric calculations
00267   kinematics_metrics::KinematicsMetricsPtr kinematics_metrics_;
00268   std::map<std::string, dynamics_solver::DynamicsSolverPtr> dynamics_solver_;
00269   boost::mutex update_metrics_lock_;
00270 
00271   // The trajectory playback component
00272   TrajectoryVisualizationPtr trajectory_visual_;
00273 
00274   // properties to show on side panel
00275   rviz::Property* path_category_;
00276   rviz::Property* plan_category_;
00277   rviz::Property* metrics_category_;
00278 
00279   rviz::EditableEnumProperty* planning_group_property_;
00280   rviz::BoolProperty* query_start_state_property_;
00281   rviz::BoolProperty* query_goal_state_property_;
00282   rviz::FloatProperty* query_marker_scale_property_;
00283   rviz::ColorProperty* query_start_color_property_;
00284   rviz::ColorProperty* query_goal_color_property_;
00285   rviz::FloatProperty* query_start_alpha_property_;
00286   rviz::FloatProperty* query_goal_alpha_property_;
00287   rviz::ColorProperty* query_colliding_link_color_property_;
00288   rviz::ColorProperty* query_outside_joint_limits_link_color_property_;
00289 
00290   rviz::BoolProperty* compute_weight_limit_property_;
00291   rviz::BoolProperty* show_manipulability_index_property_;
00292   rviz::BoolProperty* show_manipulability_property_;
00293   rviz::BoolProperty* show_joint_torques_property_;
00294   rviz::FloatProperty* metrics_set_payload_property_;
00295   rviz::FloatProperty* metrics_text_height_property_;
00296   rviz::BoolProperty* show_workspace_property_;
00297 
00298   rviz::Display* int_marker_display_;
00299 };
00300 
00301 }  // namespace moveit_rviz_plugin
00302 
00303 #endif


visualization
Author(s): Ioan Sucan , Dave Coleman , Sachin Chitta
autogenerated on Mon Jul 24 2017 02:22:13