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_FRAME_
00038 #define MOVEIT_MOTION_PLANNING_RVIZ_PLUGIN_MOTION_PLANNING_FRAME_
00039
00040 #include <QWidget>
00041 #include <QTreeWidgetItem>
00042 #include <QListWidgetItem>
00043
00044 #ifndef Q_MOC_RUN
00045 #include <moveit/macros/class_forward.h>
00046 #include <moveit/move_group_interface/move_group.h>
00047 #include <moveit/planning_scene_interface/planning_scene_interface.h>
00048 #include <moveit/planning_scene_monitor/planning_scene_monitor.h>
00049 #include <moveit/robot_interaction/robot_interaction.h>
00050 #include <moveit/robot_interaction/interaction_handler.h>
00051 #include <moveit/semantic_world/semantic_world.h>
00052 #include <interactive_markers/interactive_marker_server.h>
00053 #include <rviz/default_plugin/interactive_markers/interactive_marker.h>
00054 #include <moveit_msgs/MotionPlanRequest.h>
00055 #include <actionlib/client/simple_action_client.h>
00056 #include <object_recognition_msgs/ObjectRecognitionAction.h>
00057 #endif
00058
00059 #include <std_msgs/Bool.h>
00060 #include <std_msgs/Empty.h>
00061 #include <map>
00062 #include <string>
00063
00064 namespace rviz
00065 {
00066 class DisplayContext;
00067 }
00068
00069 namespace Ui
00070 {
00071 class MotionPlanningUI;
00072 }
00073
00074 namespace moveit_warehouse
00075 {
00076 MOVEIT_CLASS_FORWARD(PlanningSceneStorage);
00077 MOVEIT_CLASS_FORWARD(ConstraintsStorage);
00078 MOVEIT_CLASS_FORWARD(RobotStateStorage);
00079 }
00080
00081 namespace moveit_rviz_plugin
00082 {
00083 class MotionPlanningDisplay;
00084
00085 const std::string OBJECT_RECOGNITION_ACTION = "/recognize_objects";
00086
00087 static const std::string TAB_CONTEXT = "Context";
00088 static const std::string TAB_PLANNING = "Planning";
00089 static const std::string TAB_MANIPULATION = "Manipulation";
00090 static const std::string TAB_OBJECTS = "Scene Objects";
00091 static const std::string TAB_SCENES = "Stored Scenes";
00092 static const std::string TAB_STATES = "Stored States";
00093 static const std::string TAB_STATUS = "Status";
00094
00095 class MotionPlanningFrame : public QWidget
00096 {
00097 friend class MotionPlanningDisplay;
00098 Q_OBJECT
00099
00100 public:
00101 MotionPlanningFrame(MotionPlanningDisplay* pdisplay, rviz::DisplayContext* context, QWidget* parent = 0);
00102 ~MotionPlanningFrame();
00103
00104 void changePlanningGroup();
00105 void enable();
00106 void disable();
00107 void sceneUpdate(planning_scene_monitor::PlanningSceneMonitor::SceneUpdateType update_type);
00108
00109 protected:
00110 static const int ITEM_TYPE_SCENE = 1;
00111 static const int ITEM_TYPE_QUERY = 2;
00112
00113 void constructPlanningRequest(moveit_msgs::MotionPlanRequest& mreq);
00114
00115 void updateSceneMarkers(float wall_dt, float ros_dt);
00116
00117 void updateExternalCommunication();
00118
00119 MotionPlanningDisplay* planning_display_;
00120 rviz::DisplayContext* context_;
00121 Ui::MotionPlanningUI* ui_;
00122
00123 moveit::planning_interface::MoveGroupPtr move_group_;
00124 moveit::planning_interface::PlanningSceneInterfacePtr planning_scene_interface_;
00125 moveit::semantic_world::SemanticWorldPtr semantic_world_;
00126
00127 moveit::planning_interface::MoveGroup::PlanPtr current_plan_;
00128 moveit_warehouse::PlanningSceneStoragePtr planning_scene_storage_;
00129 moveit_warehouse::ConstraintsStoragePtr constraints_storage_;
00130 moveit_warehouse::RobotStateStoragePtr robot_state_storage_;
00131
00132 boost::shared_ptr<rviz::InteractiveMarker> scene_marker_;
00133
00134 typedef std::map<std::string, moveit_msgs::RobotState> RobotStateMap;
00135 typedef std::pair<std::string, moveit_msgs::RobotState> RobotStatePair;
00136 RobotStateMap robot_states_;
00137
00138 Q_SIGNALS:
00139 void planningFinished();
00140
00141 private Q_SLOTS:
00142
00143
00144 void databaseConnectButtonClicked();
00145 void publishSceneButtonClicked();
00146 void planningAlgorithmIndexChanged(int index);
00147 void resetDbButtonClicked();
00148 void approximateIKChanged(int state);
00149
00150
00151 void planButtonClicked();
00152 void executeButtonClicked();
00153 void planAndExecuteButtonClicked();
00154 void stopButtonClicked();
00155 void allowReplanningToggled(bool checked);
00156 void allowLookingToggled(bool checked);
00157 void allowExternalProgramCommunication(bool enable);
00158 void pathConstraintsIndexChanged(int index);
00159 void useStartStateButtonClicked();
00160 void useGoalStateButtonClicked();
00161 void onClearOctomapClicked();
00162
00163
00164 void importFileButtonClicked();
00165 void importUrlButtonClicked();
00166 void clearSceneButtonClicked();
00167 void sceneScaleChanged(int value);
00168 void sceneScaleStartChange();
00169 void sceneScaleEndChange();
00170 void removeObjectButtonClicked();
00171 void selectedCollisionObjectChanged();
00172 void objectPoseValueChanged(double value);
00173 void collisionObjectChanged(QListWidgetItem* item);
00174 void imProcessFeedback(visualization_msgs::InteractiveMarkerFeedback& feedback);
00175 void copySelectedCollisionObject();
00176 void exportAsTextButtonClicked();
00177 void importFromTextButtonClicked();
00178
00179
00180 void saveSceneButtonClicked();
00181 void planningSceneItemClicked();
00182 void saveQueryButtonClicked();
00183 void deleteSceneButtonClicked();
00184 void deleteQueryButtonClicked();
00185 void loadSceneButtonClicked();
00186 void loadQueryButtonClicked();
00187 void warehouseItemNameChanged(QTreeWidgetItem* item, int column);
00188
00189
00190 void loadStateButtonClicked();
00191 void saveStartStateButtonClicked();
00192 void saveGoalStateButtonClicked();
00193 void removeStateButtonClicked();
00194 void clearStatesButtonClicked();
00195 void setAsStartStateButtonClicked();
00196 void setAsGoalStateButtonClicked();
00197
00198
00199 void detectObjectsButtonClicked();
00200 void pickObjectButtonClicked();
00201 void placeObjectButtonClicked();
00202 void selectedDetectedObjectChanged();
00203 void detectedObjectChanged(QListWidgetItem* item);
00204 void selectedSupportSurfaceChanged();
00205
00206
00207 void tabChanged(int index);
00208
00209 private:
00210
00211 void computeDatabaseConnectButtonClicked();
00212 void computeDatabaseConnectButtonClickedHelper(int mode);
00213 void computeResetDbButtonClicked(const std::string& db);
00214 void populatePlannersList(const moveit_msgs::PlannerInterfaceDescription& desc);
00215
00216
00217 void computePlanButtonClicked();
00218 void computeExecuteButtonClicked();
00219 void computePlanAndExecuteButtonClicked();
00220 void computePlanAndExecuteButtonClickedDisplayHelper();
00221 void computeStopButtonClicked();
00222 void onFinishedExecution(bool success);
00223 void populateConstraintsList();
00224 void populateConstraintsList(const std::vector<std::string>& constr);
00225 void configureForPlanning();
00226 void configureWorkspace();
00227 void updateQueryStateHelper(robot_state::RobotState& state, const std::string& v);
00228 void fillStateSelectionOptions();
00229 void useStartStateButtonExec();
00230 void useGoalStateButtonExec();
00231
00232
00233 void addObject(const collision_detection::WorldPtr& world, const std::string& id, const shapes::ShapeConstPtr& shape,
00234 const Eigen::Affine3d& pose);
00235 void updateCollisionObjectPose(bool update_marker_position);
00236 void createSceneInteractiveMarker();
00237 void renameCollisionObject(QListWidgetItem* item);
00238 void attachDetachCollisionObject(QListWidgetItem* item);
00239 void populateCollisionObjectsList();
00240 void computeImportFromText(const std::string& path);
00241 void computeExportAsText(const std::string& path);
00242
00243
00244 void computeSaveSceneButtonClicked();
00245 void computeSaveQueryButtonClicked(const std::string& scene, const std::string& query_name);
00246 void computeLoadSceneButtonClicked();
00247 void computeLoadQueryButtonClicked();
00248 void populatePlanningSceneTreeView();
00249 void computeDeleteSceneButtonClicked();
00250 void computeDeleteQueryButtonClicked();
00251 void computeDeleteQueryButtonClickedHelper(QTreeWidgetItem* s);
00252 void checkPlanningSceneTreeEnabledButtons();
00253
00254
00255 void saveRobotStateButtonClicked(const robot_state::RobotState& state);
00256 void populateRobotStatesList();
00257
00258
00259 void processDetectedObjects();
00260 void updateDetectedObjectsList(const std::vector<std::string>& object_ids, const std::vector<std::string>& objects);
00261 void publishTables();
00262 void updateSupportSurfacesList();
00263 ros::Publisher object_recognition_trigger_publisher_;
00264 std::map<std::string, std::string> pick_object_name_;
00265 std::string place_object_name_;
00266 std::vector<geometry_msgs::PoseStamped> place_poses_;
00267 void pickObject();
00268 void placeObject();
00269 void triggerObjectDetection();
00270 void updateTables();
00271 std::string support_surface_name_;
00272
00273 std::string selected_object_name_;
00274 std::string selected_support_surface_name_;
00275
00276 boost::scoped_ptr<actionlib::SimpleActionClient<object_recognition_msgs::ObjectRecognitionAction> >
00277 object_recognition_client_;
00278 template <typename T>
00279 void waitForAction(const T& action, const ros::NodeHandle& node_handle, const ros::Duration& wait_for_server,
00280 const std::string& name);
00281 void listenDetectedObjects(const object_recognition_msgs::RecognizedObjectArrayPtr& msg);
00282 ros::Subscriber object_recognition_subscriber_;
00283
00284 ros::Subscriber plan_subscriber_;
00285 ros::Subscriber execute_subscriber_;
00286 ros::Subscriber update_start_state_subscriber_;
00287 ros::Subscriber update_goal_state_subscriber_;
00288
00289 void changePlanningGroupHelper();
00290 void importResource(const std::string& path);
00291 void loadStoredStates(const std::string& pattern);
00292
00293 void remotePlanCallback(const std_msgs::EmptyConstPtr& msg);
00294 void remoteExecuteCallback(const std_msgs::EmptyConstPtr& msg);
00295 void remoteUpdateStartStateCallback(const std_msgs::EmptyConstPtr& msg);
00296 void remoteUpdateGoalStateCallback(const std_msgs::EmptyConstPtr& msg);
00297
00298
00299 void setItemSelectionInList(const std::string& item_name, bool selection, QListWidget* list);
00300
00301 ros::NodeHandle nh_;
00302 ros::Publisher planning_scene_publisher_;
00303 ros::Publisher planning_scene_world_publisher_;
00304
00305 collision_detection::CollisionWorld::ObjectConstPtr scaled_object_;
00306
00307 std::vector<std::pair<std::string, bool> > known_collision_objects_;
00308 long unsigned int known_collision_objects_version_;
00309 bool first_time_;
00310 ros::ServiceClient clear_octomap_service_client_;
00311 };
00312
00313
00314 template <typename T>
00315 void MotionPlanningFrame::waitForAction(const T& action, const ros::NodeHandle& node_handle,
00316 const ros::Duration& wait_for_server, const std::string& name)
00317 {
00318 ROS_DEBUG("Waiting for MoveGroup action server (%s)...", name.c_str());
00319
00320
00321 ros::Time start_time = ros::Time::now();
00322 while (start_time == ros::Time::now())
00323 {
00324 ros::WallDuration(0.01).sleep();
00325 ros::spinOnce();
00326 }
00327
00328
00329 if (wait_for_server == ros::Duration(0, 0))
00330 {
00331
00332 while (node_handle.ok() && !action->isServerConnected())
00333 {
00334 ros::WallDuration(0.02).sleep();
00335 ros::spinOnce();
00336 }
00337 }
00338 else
00339 {
00340
00341 ros::WallTime final_time = ros::WallTime::now() + ros::WallDuration(wait_for_server.toSec());
00342 while (node_handle.ok() && !action->isServerConnected() && final_time > ros::WallTime::now())
00343 {
00344 ros::WallDuration(0.02).sleep();
00345 ros::spinOnce();
00346 }
00347 }
00348
00349 if (!action->isServerConnected())
00350 throw std::runtime_error("Unable to connect to move_group action server within allotted time");
00351 else
00352 ROS_DEBUG("Connected to '%s'", name.c_str());
00353 };
00354 }
00355
00356 #endif