Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032 #ifndef MOVEIT_MOTION_PLANNING_RVIZ_PLUGIN_MOTION_PLANNING_DISPLAY_
00033 #define MOVEIT_MOTION_PLANNING_RVIZ_PLUGIN_MOTION_PLANNING_DISPLAY_
00034
00035 #include <rviz/display.h>
00036 #include <rviz/selection/selection_manager.h>
00037 #include <moveit/planning_scene_rviz_plugin/planning_scene_display.h>
00038
00039 #ifndef Q_MOC_RUN
00040 #include <moveit/motion_planning_rviz_plugin/motion_planning_frame.h>
00041 #include <moveit/robot_interaction/robot_interaction.h>
00042
00043 #include <moveit/planning_scene_monitor/planning_scene_monitor.h>
00044 #include <moveit/kinematics_metrics/kinematics_metrics.h>
00045 #include <moveit/dynamics_solver/dynamics_solver.h>
00046 #include <ros/ros.h>
00047 #endif
00048
00049 #include <moveit_msgs/DisplayTrajectory.h>
00050 #include <QDockWidget>
00051
00052 namespace Ogre
00053 {
00054 class SceneNode;
00055 }
00056
00057 namespace rviz
00058 {
00059 class Robot;
00060 class Shape;
00061 class Property;
00062 class StringProperty;
00063 class BoolProperty;
00064 class FloatProperty;
00065 class RosTopicProperty;
00066 class EditableEnumProperty;
00067 class ColorProperty;
00068 class MovableText;
00069 }
00070
00071 namespace moveit_rviz_plugin
00072 {
00073
00074 class MotionPlanningDisplay : public PlanningSceneDisplay
00075 {
00076 Q_OBJECT
00077
00078 public:
00079
00080 MotionPlanningDisplay();
00081
00082 virtual ~MotionPlanningDisplay();
00083
00084 virtual void load( const rviz::Config& config );
00085 virtual void save( rviz::Config config ) const;
00086
00087 virtual void update(float wall_dt, float ros_dt);
00088 virtual void reset();
00089
00090 robot_state::RobotStateConstPtr getQueryStartState() const
00091 {
00092 return query_start_state_->getState();
00093 }
00094
00095 robot_state::RobotStateConstPtr getQueryGoalState() const
00096 {
00097 return query_goal_state_->getState();
00098 }
00099
00100 const robot_interaction::RobotInteractionPtr& getRobotInteraction() const
00101 {
00102 return robot_interaction_;
00103 }
00104
00105 const robot_interaction::RobotInteraction::InteractionHandlerPtr& getQueryStartStateHandler() const
00106 {
00107 return query_start_state_;
00108 }
00109
00110 const robot_interaction::RobotInteraction::InteractionHandlerPtr& getQueryGoalStateHandler() const
00111 {
00112 return query_goal_state_;
00113 }
00114
00115 void setQueryStartState(const robot_state::RobotState &start);
00116 void setQueryGoalState(const robot_state::RobotState &goal);
00117
00118 void updateQueryStartState();
00119 void updateQueryGoalState();
00120
00121 void useApproximateIK(bool flag);
00122
00123 std::string getCurrentPlanningGroup() const;
00124
00125 void changePlanningGroup(const std::string& group);
00126
00127 void addStatusText(const std::string &text);
00128 void addStatusText(const std::vector<std::string> &text);
00129 void setStatusTextColor(const QColor &color);
00130 void resetStatusTextColor();
00131
00132 private Q_SLOTS:
00133
00134
00135
00136
00137
00138 void changedDisplayPathVisualEnabled();
00139 void changedDisplayPathCollisionEnabled();
00140 void changedRobotPathAlpha();
00141 void changedStateDisplayTime();
00142 void changedLoopDisplay();
00143 void changedShowTrail();
00144 void changedTrajectoryTopic();
00145 void changedQueryStartState();
00146 void changedQueryGoalState();
00147 void changedQueryMarkerScale();
00148 void changedQueryStartColor();
00149 void changedQueryGoalColor();
00150 void changedQueryStartAlpha();
00151 void changedQueryGoalAlpha();
00152 void changedQueryCollidingLinkColor();
00153 void changedQueryJointViolationColor();
00154 void changedPlanningGroup();
00155 void changedShowWeightLimit();
00156 void changedShowManipulabilityIndex();
00157 void changedShowManipulability();
00158 void changedShowJointTorques();
00159 void changedMetricsSetPayload();
00160 void changedMetricsTextHeight();
00161 void changedWorkspace();
00162 void resetInteractiveMarkers();
00163
00164 protected:
00165
00166 virtual void onRobotModelLoaded();
00167 virtual void onSceneMonitorReceivedUpdate(planning_scene_monitor::PlanningSceneMonitor::SceneUpdateType update_type);
00168 virtual void updateInternal(float wall_dt, float ros_dt);
00169
00173 void incomingDisplayTrajectory(const moveit_msgs::DisplayTrajectory::ConstPtr& msg);
00174
00175 void renderWorkspaceBox();
00176 void updateLinkColors();
00177
00178 void displayTable(const std::map<std::string, double> &values,
00179 const Ogre::ColourValue &color,
00180 const Ogre::Vector3 &pos, const Ogre::Quaternion &orient);
00181 void displayMetrics(bool start);
00182
00183 void executeMainLoopJobs();
00184 void clearTrajectoryTrail();
00185 void publishInteractiveMarkers(bool pose_update);
00186
00187 void recomputeQueryStartStateMetrics();
00188 void recomputeQueryGoalStateMetrics();
00189 void drawQueryStartState();
00190 void drawQueryGoalState();
00191 void scheduleDrawQueryStartState(robot_interaction::RobotInteraction::InteractionHandler *handler, bool error_state_changed);
00192 void scheduleDrawQueryGoalState(robot_interaction::RobotInteraction::InteractionHandler *handler, bool error_state_changed);
00193
00194 bool isIKSolutionCollisionFree(robot_state::JointStateGroup *group, const std::vector<double> &ik_solution) const;
00195
00196 void computeMetrics(bool start, const std::string &group, double payload);
00197 void computeMetricsInternal(std::map<std::string, double> &metrics,
00198 const robot_interaction::RobotInteraction::EndEffector &eef,
00199 const robot_state::RobotState &state, double payload);
00200 void updateStateExceptModified(robot_state::RobotState &dest, const robot_state::RobotState &src);
00201 float getStateDisplayTime();
00202 void updateBackgroundJobProgressBar();
00203 void backgroundJobUpdate(BackgroundProcessing::JobEvent event);
00204
00205 void setQueryStateHelper(bool use_start_state, const std::string &v);
00206 void populateMenuHandler(boost::shared_ptr<interactive_markers::MenuHandler>& mh);
00207
00208
00209 virtual void onInitialize();
00210 virtual void onEnable();
00211 virtual void onDisable();
00212 virtual void fixedFrameChanged();
00213
00214
00215 RobotStateVisualizationPtr query_robot_start_;
00216 RobotStateVisualizationPtr query_robot_goal_;
00217 RobotStateVisualizationPtr display_path_robot_;
00218
00219 Ogre::SceneNode* text_display_scene_node_;
00220 bool text_display_for_start_;
00221 rviz::MovableText *text_to_display_;
00222
00223 robot_trajectory::RobotTrajectoryPtr displaying_trajectory_message_;
00224 robot_trajectory::RobotTrajectoryPtr trajectory_message_to_display_;
00225 std::vector<rviz::Robot*> trajectory_trail_;
00226 ros::Subscriber trajectory_topic_sub_;
00227 ros::NodeHandle private_handle_;
00228 bool animating_path_;
00229 int current_state_;
00230 float current_state_time_;
00231
00232
00233 boost::scoped_ptr<rviz::Shape> workspace_box_;
00234
00235
00236 MotionPlanningFrame *frame_;
00237 QDockWidget *frame_dock_;
00238
00239
00240 robot_interaction::RobotInteractionPtr robot_interaction_;
00241 robot_interaction::RobotInteraction::InteractionHandlerPtr query_start_state_;
00242 robot_interaction::RobotInteraction::InteractionHandlerPtr query_goal_state_;
00243 boost::shared_ptr<interactive_markers::MenuHandler> menu_handler_start_;
00244 boost::shared_ptr<interactive_markers::MenuHandler> menu_handler_goal_;
00245 std::map<std::string, int> collision_links_start_;
00246 std::map<std::string, int> collision_links_goal_;
00248 std::set<std::string> modified_groups_;
00249
00252 std::map<std::pair<bool, std::string>, std::map<std::string, double> > computed_metrics_;
00254 std::map<std::string, bool> position_only_ik_;
00255
00256
00257 kinematics_metrics::KinematicsMetricsPtr kinematics_metrics_;
00258 std::map<std::string, dynamics_solver::DynamicsSolverPtr> dynamics_solver_;
00259 boost::mutex update_metrics_lock_;
00260
00261
00262 rviz::Property* path_category_;
00263 rviz::Property* plan_category_;
00264 rviz::Property* metrics_category_;
00265
00266 rviz::EditableEnumProperty* planning_group_property_;
00267 rviz::BoolProperty* query_start_state_property_;
00268 rviz::BoolProperty* query_goal_state_property_;
00269 rviz::FloatProperty* query_marker_scale_property_;
00270 rviz::ColorProperty* query_start_color_property_;
00271 rviz::ColorProperty* query_goal_color_property_;
00272 rviz::FloatProperty* query_start_alpha_property_;
00273 rviz::FloatProperty* query_goal_alpha_property_;
00274 rviz::ColorProperty* query_colliding_link_color_property_;
00275 rviz::ColorProperty* query_outside_joint_limits_link_color_property_;
00276
00277 rviz::BoolProperty* display_path_visual_enabled_property_;
00278 rviz::BoolProperty* display_path_collision_enabled_property_;
00279 rviz::EditableEnumProperty* state_display_time_property_;
00280 rviz::RosTopicProperty* trajectory_topic_property_;
00281 rviz::FloatProperty* robot_path_alpha_property_;
00282 rviz::BoolProperty* loop_display_property_;
00283 rviz::BoolProperty* trail_display_property_;
00284 rviz::BoolProperty* compute_weight_limit_property_;
00285 rviz::BoolProperty* show_manipulability_index_property_;
00286 rviz::BoolProperty* show_manipulability_property_;
00287 rviz::BoolProperty* show_joint_torques_property_;
00288 rviz::FloatProperty* metrics_set_payload_property_;
00289 rviz::FloatProperty* metrics_text_height_property_;
00290 rviz::BoolProperty* show_workspace_property_;
00291
00292 rviz::Display *int_marker_display_;
00293 };
00294
00295 }
00296
00297 #endif