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
00230
00231 void addObject(const collision_detection::WorldPtr& world, const std::string& id, const shapes::ShapeConstPtr& shape,
00232 const Eigen::Affine3d& pose);
00233 void updateCollisionObjectPose(bool update_marker_position);
00234 void createSceneInteractiveMarker();
00235 void renameCollisionObject(QListWidgetItem* item);
00236 void attachDetachCollisionObject(QListWidgetItem* item);
00237 void populateCollisionObjectsList();
00238 void computeImportFromText(const std::string& path);
00239 void computeExportAsText(const std::string& path);
00240
00241
00242 void computeSaveSceneButtonClicked();
00243 void computeSaveQueryButtonClicked(const std::string& scene, const std::string& query_name);
00244 void computeLoadSceneButtonClicked();
00245 void computeLoadQueryButtonClicked();
00246 void populatePlanningSceneTreeView();
00247 void computeDeleteSceneButtonClicked();
00248 void computeDeleteQueryButtonClicked();
00249 void computeDeleteQueryButtonClickedHelper(QTreeWidgetItem* s);
00250 void checkPlanningSceneTreeEnabledButtons();
00251
00252
00253 void saveRobotStateButtonClicked(const robot_state::RobotState& state);
00254 void populateRobotStatesList();
00255
00256
00257 void processDetectedObjects();
00258 void updateDetectedObjectsList(const std::vector<std::string>& object_ids, const std::vector<std::string>& objects);
00259 void publishTables();
00260 void updateSupportSurfacesList();
00261 ros::Publisher object_recognition_trigger_publisher_;
00262 std::map<std::string, std::string> pick_object_name_;
00263 std::string place_object_name_;
00264 std::vector<geometry_msgs::PoseStamped> place_poses_;
00265 void pickObject();
00266 void placeObject();
00267 void triggerObjectDetection();
00268 void updateTables();
00269 std::string support_surface_name_;
00270
00271 std::string selected_object_name_;
00272 std::string selected_support_surface_name_;
00273
00274 boost::scoped_ptr<actionlib::SimpleActionClient<object_recognition_msgs::ObjectRecognitionAction> >
00275 object_recognition_client_;
00276 template <typename T>
00277 void waitForAction(const T& action, const ros::NodeHandle& node_handle, const ros::Duration& wait_for_server,
00278 const std::string& name);
00279 void listenDetectedObjects(const object_recognition_msgs::RecognizedObjectArrayPtr& msg);
00280 ros::Subscriber object_recognition_subscriber_;
00281
00282 ros::Subscriber plan_subscriber_;
00283 ros::Subscriber execute_subscriber_;
00284 ros::Subscriber update_start_state_subscriber_;
00285 ros::Subscriber update_goal_state_subscriber_;
00286
00287 void changePlanningGroupHelper();
00288 void importResource(const std::string& path);
00289 void loadStoredStates(const std::string& pattern);
00290
00291 void remotePlanCallback(const std_msgs::EmptyConstPtr& msg);
00292 void remoteExecuteCallback(const std_msgs::EmptyConstPtr& msg);
00293 void remoteUpdateStartStateCallback(const std_msgs::EmptyConstPtr& msg);
00294 void remoteUpdateGoalStateCallback(const std_msgs::EmptyConstPtr& msg);
00295
00296
00297 void setItemSelectionInList(const std::string& item_name, bool selection, QListWidget* list);
00298
00299 ros::NodeHandle nh_;
00300 ros::Publisher planning_scene_publisher_;
00301 ros::Publisher planning_scene_world_publisher_;
00302
00303 collision_detection::CollisionWorld::ObjectConstPtr scaled_object_;
00304
00305 std::vector<std::pair<std::string, bool> > known_collision_objects_;
00306 long unsigned int known_collision_objects_version_;
00307 bool first_time_;
00308 ros::ServiceClient clear_octomap_service_client_;
00309 };
00310
00311
00312 template <typename T>
00313 void MotionPlanningFrame::waitForAction(const T& action, const ros::NodeHandle& node_handle,
00314 const ros::Duration& wait_for_server, const std::string& name)
00315 {
00316 ROS_DEBUG("Waiting for MoveGroup action server (%s)...", name.c_str());
00317
00318
00319 ros::Time start_time = ros::Time::now();
00320 while (start_time == ros::Time::now())
00321 {
00322 ros::WallDuration(0.01).sleep();
00323 ros::spinOnce();
00324 }
00325
00326
00327 if (wait_for_server == ros::Duration(0, 0))
00328 {
00329
00330 while (node_handle.ok() && !action->isServerConnected())
00331 {
00332 ros::WallDuration(0.02).sleep();
00333 ros::spinOnce();
00334 }
00335 }
00336 else
00337 {
00338
00339 ros::WallTime final_time = ros::WallTime::now() + ros::WallDuration(wait_for_server.toSec());
00340 while (node_handle.ok() && !action->isServerConnected() && final_time > ros::WallTime::now())
00341 {
00342 ros::WallDuration(0.02).sleep();
00343 ros::spinOnce();
00344 }
00345 }
00346
00347 if (!action->isServerConnected())
00348 throw std::runtime_error("Unable to connect to move_group action server within allotted time");
00349 else
00350 ROS_DEBUG("Connected to '%s'", name.c_str());
00351 };
00352 }
00353
00354 #endif