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_MOVEIT_WAREHOUSE_PLANNING_SCENE_STORAGE_
00038 #define MOVEIT_MOVEIT_WAREHOUSE_PLANNING_SCENE_STORAGE_
00039
00040 #include "moveit/warehouse/moveit_message_storage.h"
00041 #include <moveit_msgs/PlanningScene.h>
00042 #include <moveit_msgs/MotionPlanRequest.h>
00043 #include <moveit_msgs/RobotTrajectory.h>
00044
00045 namespace moveit_warehouse
00046 {
00047
00048 typedef mongo_ros::MessageWithMetadata<moveit_msgs::PlanningScene>::ConstPtr PlanningSceneWithMetadata;
00049 typedef mongo_ros::MessageWithMetadata<moveit_msgs::MotionPlanRequest>::ConstPtr MotionPlanRequestWithMetadata;
00050 typedef mongo_ros::MessageWithMetadata<moveit_msgs::RobotTrajectory>::ConstPtr RobotTrajectoryWithMetadata;
00051
00052 typedef boost::shared_ptr<mongo_ros::MessageCollection<moveit_msgs::PlanningScene> > PlanningSceneCollection;
00053 typedef boost::shared_ptr<mongo_ros::MessageCollection<moveit_msgs::MotionPlanRequest> > MotionPlanRequestCollection;
00054 typedef boost::shared_ptr<mongo_ros::MessageCollection<moveit_msgs::RobotTrajectory> > RobotTrajectoryCollection;
00055
00056 class PlanningSceneStorage : public MoveItMessageStorage
00057 {
00058 public:
00059
00060 static const std::string DATABASE_NAME;
00061
00062 static const std::string PLANNING_SCENE_ID_NAME;
00063 static const std::string MOTION_PLAN_REQUEST_ID_NAME;
00064
00070 PlanningSceneStorage(const std::string &host = "", const unsigned int port = 0, double wait_seconds = 5.0);
00071
00072 void addPlanningScene(const moveit_msgs::PlanningScene &scene);
00073 void addPlanningQuery(const moveit_msgs::MotionPlanRequest &planning_query, const std::string &scene_name, const std::string &query_name = "");
00074 void addPlanningResult(const moveit_msgs::MotionPlanRequest &planning_query, const moveit_msgs::RobotTrajectory &result, const std::string &scene_name);
00075
00076 bool hasPlanningScene(const std::string &name) const;
00077 void getPlanningSceneNames(std::vector<std::string> &names) const;
00078 void getPlanningSceneNames(const std::string ®ex, std::vector<std::string> &names) const;
00079
00081 bool getPlanningScene(PlanningSceneWithMetadata &scene_m, const std::string &scene_name) const;
00082 bool getPlanningSceneWorld(moveit_msgs::PlanningSceneWorld &world, const std::string &scene_name) const;
00083
00084 bool hasPlanningQuery(const std::string &scene_name, const std::string &query_name) const;
00085 bool getPlanningQuery(MotionPlanRequestWithMetadata &query_m, const std::string &scene_name, const std::string &query_name);
00086 void getPlanningQueries(std::vector<MotionPlanRequestWithMetadata> &planning_queries, const std::string &scene_name) const;
00087 void getPlanningQueriesNames(std::vector<std::string> &query_names, const std::string &scene_name) const;
00088 void getPlanningQueriesNames(const std::string ®ex, std::vector<std::string> &query_names, const std::string &scene_name) const;
00089 void getPlanningQueries(std::vector<MotionPlanRequestWithMetadata> &planning_queries, std::vector<std::string> &query_names, const std::string &scene_name) const;
00090
00091 void getPlanningResults(std::vector<RobotTrajectoryWithMetadata> &planning_results, const std::string &scene_name, const moveit_msgs::MotionPlanRequest &planning_query) const;
00092 void getPlanningResults(std::vector<RobotTrajectoryWithMetadata> &planning_results, const std::string &scene_name, const std::string &query_name) const;
00093
00094 void renamePlanningScene(const std::string &old_scene_name, const std::string &new_scene_name);
00095 void renamePlanningQuery(const std::string &scene_name, const std::string &old_query_name, const std::string &new_query_name);
00096
00097 void removePlanningScene(const std::string &scene_name);
00098 void removePlanningQuery(const std::string &scene_name, const std::string &query_name);
00099 void removePlanningQueries(const std::string &scene_name);
00100 void removePlanningResults(const std::string &scene_name);
00101 void removePlanningResults(const std::string &scene_name, const std::string &query_name);
00102
00103 void reset();
00104
00105 private:
00106
00107 void createCollections();
00108
00109 std::string getMotionPlanRequestName(const moveit_msgs::MotionPlanRequest &planning_query, const std::string &scene_name) const;
00110 std::string addNewPlanningRequest(const moveit_msgs::MotionPlanRequest &planning_query, const std::string &scene_name, const std::string &query_name);
00111
00112 PlanningSceneCollection planning_scene_collection_;
00113 MotionPlanRequestCollection motion_plan_request_collection_;
00114 RobotTrajectoryCollection robot_trajectory_collection_;
00115 };
00116 }
00117
00118 #endif