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
00033
00034
00035
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
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
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
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
00241 boost::scoped_ptr<rviz::Shape> workspace_box_;
00242
00243
00244 MotionPlanningFrame* frame_;
00245 rviz::PanelDockWidget* frame_dock_;
00246
00247
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
00267 kinematics_metrics::KinematicsMetricsPtr kinematics_metrics_;
00268 std::map<std::string, dynamics_solver::DynamicsSolverPtr> dynamics_solver_;
00269 boost::mutex update_metrics_lock_;
00270
00271
00272 TrajectoryVisualizationPtr trajectory_visual_;
00273
00274
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 }
00302
00303 #endif