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/macros/class_forward.h>
00042 #include <moveit_msgs/PlanningScene.h>
00043 #include <moveit_msgs/MotionPlanRequest.h>
00044 #include <moveit_msgs/RobotTrajectory.h>
00045
00046 namespace moveit_warehouse
00047 {
00048 typedef warehouse_ros::MessageWithMetadata<moveit_msgs::PlanningScene>::ConstPtr PlanningSceneWithMetadata;
00049 typedef warehouse_ros::MessageWithMetadata<moveit_msgs::MotionPlanRequest>::ConstPtr MotionPlanRequestWithMetadata;
00050 typedef warehouse_ros::MessageWithMetadata<moveit_msgs::RobotTrajectory>::ConstPtr RobotTrajectoryWithMetadata;
00051
00052 typedef warehouse_ros::MessageCollection<moveit_msgs::PlanningScene>::Ptr PlanningSceneCollection;
00053 typedef warehouse_ros::MessageCollection<moveit_msgs::MotionPlanRequest>::Ptr MotionPlanRequestCollection;
00054 typedef warehouse_ros::MessageCollection<moveit_msgs::RobotTrajectory>::Ptr RobotTrajectoryCollection;
00055
00056 MOVEIT_CLASS_FORWARD(PlanningSceneStorage);
00057
00058 class PlanningSceneStorage : public MoveItMessageStorage
00059 {
00060 public:
00061 static const std::string DATABASE_NAME;
00062
00063 static const std::string PLANNING_SCENE_ID_NAME;
00064 static const std::string MOTION_PLAN_REQUEST_ID_NAME;
00065
00066 PlanningSceneStorage(warehouse_ros::DatabaseConnection::Ptr conn);
00067
00068 void addPlanningScene(const moveit_msgs::PlanningScene& scene);
00069 void addPlanningQuery(const moveit_msgs::MotionPlanRequest& planning_query, const std::string& scene_name,
00070 const std::string& query_name = "");
00071 void addPlanningResult(const moveit_msgs::MotionPlanRequest& planning_query,
00072 const moveit_msgs::RobotTrajectory& result, const std::string& scene_name);
00073
00074 bool hasPlanningScene(const std::string& name) const;
00075 void getPlanningSceneNames(std::vector<std::string>& names) const;
00076 void getPlanningSceneNames(const std::string& regex, std::vector<std::string>& names) const;
00077
00079 bool getPlanningScene(PlanningSceneWithMetadata& scene_m, const std::string& scene_name) const;
00080 bool getPlanningSceneWorld(moveit_msgs::PlanningSceneWorld& world, const std::string& scene_name) const;
00081
00082 bool hasPlanningQuery(const std::string& scene_name, const std::string& query_name) const;
00083 bool getPlanningQuery(MotionPlanRequestWithMetadata& query_m, const std::string& scene_name,
00084 const std::string& query_name);
00085 void getPlanningQueries(std::vector<MotionPlanRequestWithMetadata>& planning_queries,
00086 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& regex, std::vector<std::string>& query_names,
00089 const std::string& scene_name) const;
00090 void getPlanningQueries(std::vector<MotionPlanRequestWithMetadata>& planning_queries,
00091 std::vector<std::string>& query_names, const std::string& scene_name) const;
00092
00093 void getPlanningResults(std::vector<RobotTrajectoryWithMetadata>& planning_results, const std::string& scene_name,
00094 const moveit_msgs::MotionPlanRequest& planning_query) const;
00095 void getPlanningResults(std::vector<RobotTrajectoryWithMetadata>& planning_results, const std::string& scene_name,
00096 const std::string& query_name) const;
00097
00098 void renamePlanningScene(const std::string& old_scene_name, const std::string& new_scene_name);
00099 void renamePlanningQuery(const std::string& scene_name, const std::string& old_query_name,
00100 const std::string& new_query_name);
00101
00102 void removePlanningScene(const std::string& scene_name);
00103 void removePlanningQuery(const std::string& scene_name, const std::string& query_name);
00104 void removePlanningQueries(const std::string& scene_name);
00105 void removePlanningResults(const std::string& scene_name);
00106 void removePlanningResults(const std::string& scene_name, const std::string& query_name);
00107
00108 void reset();
00109
00110 private:
00111 void createCollections();
00112
00113 std::string getMotionPlanRequestName(const moveit_msgs::MotionPlanRequest& planning_query,
00114 const std::string& scene_name) const;
00115 std::string addNewPlanningRequest(const moveit_msgs::MotionPlanRequest& planning_query, const std::string& scene_name,
00116 const std::string& query_name);
00117
00118 PlanningSceneCollection planning_scene_collection_;
00119 MotionPlanRequestCollection motion_plan_request_collection_;
00120 RobotTrajectoryCollection robot_trajectory_collection_;
00121 };
00122 }
00123
00124 #endif